diff --git a/OdeWorld.cc b/OdeWorld.cc index 032c665..f08a46a 100644 --- a/OdeWorld.cc +++ b/OdeWorld.cc @@ -7,26 +7,6 @@ using namespace std; #define WORLD_STEP 0.001 -#define WHITESPACE " \t\r\n\f" - -static string trim(const string & orig) -{ - string result = orig; - size_t pos = result.find_first_not_of(WHITESPACE); - if (pos == string::npos) - { - result = ""; - } - else - { - if (pos > 0) - result = result.substr(pos, result.length() - pos); - pos = result.find_last_not_of(WHITESPACE); - if (pos < result.length() - 1) - result = result.substr(0, pos + 1); - } - return result; -} /* used by ODE to perform collision detection */ void OdeWorld_collide_callback(void * data, dGeomID o1, dGeomID o2) @@ -269,73 +249,10 @@ dGeomID OdeWorld::Object::cloneGeom(dGeomID geom, dBodyID body) return id; } -void OdeWorld::Object::loadPhy(const std::string & path) -{ - ifstream ifs(path.c_str()); - if (ifs.is_open()) - { - while (!ifs.eof()) - { - string line; - getline(ifs, line); - line = trim(line); - if (line == "" || line[0] == '#') - continue; - size_t pos = line.find_first_of(WHITESPACE); - if (pos == string::npos) - continue; - string type = line.substr(0, pos); - pos = line.find("\"", pos); - if (pos == string::npos) - continue; - size_t pos2 = line.find("\"", pos + 1); - if (pos2 == string::npos) - continue; - string name = line.substr(pos + 1, pos2 - pos - 1); - pos = pos2 + 1; - vector args; - for (;;) - { - pos = line.find_first_not_of(WHITESPACE, pos); - if (pos == string::npos) - break; - pos2 = line.find_first_of(WHITESPACE, pos); - string n = line.substr(pos, pos2 - pos); - float f = atof(n.c_str()); - args.push_back(f); - if (pos2 == string::npos) - break; - pos = pos2 + 1; - } - if (type == "cube") - { - addCube(args); - } - else if (type == "sphere") - { - addSphere(args); - } - else if (type == "cylinder") - { - addCylinder(args); - } - else if (type == "plane") - { - addPlane(args); - } - } - } - if (m_body != 0) - { - dMassTranslate(&m_mass, -m_mass.c[0], -m_mass.c[1], -m_mass.c[2]); - dBodySetMass(m_body, &m_mass); - } -} - -void OdeWorld::Object::addCube(const vector args) +bool OdeWorld::Object::addCube(const vector args) { if (args.size() != 9) - return; + return false; dGeomID id = dCreateBox(0, m_scale * args[0], m_scale * args[1], @@ -348,48 +265,52 @@ void OdeWorld::Object::addCube(const vector args) setupGeom(id, &mass, args[3], args[4], args[5], args[6], args[7], args[8]); + return true; } -void OdeWorld::Object::addSphere(const vector args) +bool OdeWorld::Object::addSphere(const vector args) { if (args.size() != 4) - return; + return false; dGeomID id = dCreateSphere(0, m_scale * args[0]); dMass mass; dMassSetSphere(&mass, 1.0, m_scale * args[0]); setupGeom(id, &mass, args[1], args[2], args[3], 0.0, 0.0, 0.0); + return true; } -void OdeWorld::Object::addCylinder(const vector args) +bool OdeWorld::Object::addCylinder(const vector args) { if (args.size() != 8) - return; + return false; dGeomID id = dCreateCylinder(0, m_scale * args[0], m_scale * args[1]); dMass mass; dMassSetCylinder(&mass, 1.0, 3, m_scale * args[0], m_scale * args[1]); setupGeom(id, &mass, args[2], args[3], args[4], args[5], args[6], args[7]); + return true; } -void OdeWorld::Object::addCCylinder(const vector args) +bool OdeWorld::Object::addCCylinder(const vector args) { if (args.size() != 8) - return; + return false; dGeomID id = dCreateCCylinder(0, m_scale * args[0], m_scale * args[1]); dMass mass; dMassSetCappedCylinder(&mass, 1.0, 3, m_scale * args[0], m_scale * args[1]); setupGeom(id, &mass, args[2], args[3], args[4], args[5], args[6], args[7]); + return true; } -void OdeWorld::Object::addPlane(const vector args) +bool OdeWorld::Object::addPlane(const vector args) { if (args.size() != 6) - return; + return false; dMatrix3 r; dRFromEulerAngles(r, args[3], args[4], args[5]); @@ -404,6 +325,7 @@ void OdeWorld::Object::addPlane(const vector args) dGeomID id = dCreatePlane(m_space, a, b, c, d); m_geoms.push_back(id); + return true; } void OdeWorld::Object::setupGeom(dGeomID geom, dMass * mass, @@ -550,3 +472,12 @@ void OdeWorld::Object::addRelTorque(dReal fx, dReal fy, dReal fz) if (m_body != 0) dBodyAddRelTorque(m_body, fx, fy, fz); } + +void OdeWorld::Object::finalize() +{ + if (m_body != 0) + { + dMassTranslate(&m_mass, -m_mass.c[0], -m_mass.c[1], -m_mass.c[2]); + dBodySetMass(m_body, &m_mass); + } +} diff --git a/OdeWorld.h b/OdeWorld.h index 593c61c..97b3d66 100644 --- a/OdeWorld.h +++ b/OdeWorld.h @@ -21,23 +21,21 @@ class OdeWorld Object(const Object & orig); ~Object(); - void loadPhy(const std::string & path); void setPosition(double x, double y, double z); void getPosition(double * x, double * y, double * z); void setRotation(dReal x, dReal y, dReal z); const dReal * getPosition(); const dReal * getRotation(); - std::vector loadPhy(const std::string & path, - dBodyID * body, bool is_static = false); - void addCube(const std::vector args); - void addSphere(const std::vector args); - void addCylinder(const std::vector args); - void addCCylinder(const std::vector args); - void addPlane(const std::vector args); + bool addCube(const std::vector args); + bool addSphere(const std::vector args); + bool addCylinder(const std::vector args); + bool addCCylinder(const std::vector args); + bool addPlane(const std::vector args); void addForce(dReal fx, dReal fy, dReal fz); void addRelForce(dReal fx, dReal fy, dReal fz); void addTorque(dReal fx, dReal fy, dReal fz); void addRelTorque(dReal fx, dReal fy, dReal fz); + void finalize(); protected: bool m_is_static;