From 3c6d089a4722a0dbeb1a60217e4967986ad1e4fe Mon Sep 17 00:00:00 2001 From: josh Date: Thu, 8 Oct 2009 03:50:33 +0000 Subject: [PATCH] refactored geometry addition classes into OdeWorld::Object git-svn-id: svn://anubis/misc/OdeWorld@161 bd8a9e45-a331-0410-811e-c64571078777 --- OdeWorld.cc | 212 +++++++++++++++++++++++++--------------------------- OdeWorld.h | 33 ++++---- 2 files changed, 116 insertions(+), 129 deletions(-) diff --git a/OdeWorld.cc b/OdeWorld.cc index 5863a67..acacaa0 100644 --- a/OdeWorld.cc +++ b/OdeWorld.cc @@ -80,11 +80,76 @@ void OdeWorld::step() dJointGroupEmpty(m_contactJointGroup); } -vector OdeWorld::loadPhy(const std::string & path, - dBodyID * body, bool is_static) +/* push an OpenGL matrix onto the matrix stack for a given + * ODE body position and rotation */ +void OdeWorld::pushTransform(const float pos[3], const float R[12]) { - vector ret; + GLfloat matrix[16]; + matrix[0] = R[0]; + matrix[1] = R[4]; + matrix[2] = R[8]; + matrix[3] = 0; + matrix[4] = R[1]; + matrix[5] = R[5]; + matrix[6] = R[9]; + matrix[7] = 0; + matrix[8] = R[2]; + matrix[9] = R[6]; + matrix[10] = R[10]; + matrix[11] = 0; + matrix[12] = pos[0]; + matrix[13] = pos[1]; + matrix[14] = pos[2]; + matrix[15] = 1; + glPushMatrix(); + glMultMatrixf(matrix); +} +OdeWorld::Object::Object(bool is_static, dWorldID world, dSpaceID space) +{ + m_is_static = is_static; + m_world = world; + m_space = space; +} + +void OdeWorld::Object::setPosition(double x, double y, double z) +{ + if (m_is_static) + { + for (int i = 0, sz = m_geoms.size(); i < sz; i++) + dGeomSetPosition(m_geoms[i], x, y, z); + } + else + { + if (m_body != 0) + dBodySetPosition(m_body, x, y, z); + } +} + +void OdeWorld::Object::getPosition(double * x, double * y, double * z) +{ + const dReal * pos = NULL; + if (m_is_static) + { + if (m_geoms.size() > 0) + pos = dGeomGetPosition(m_geoms[0]); + } + else + { + if (m_body != 0) + pos = dBodyGetPosition(m_body); + } + + if (pos != NULL) + { + *x = pos[0]; + *y = pos[1]; + *z = pos[2]; + } +} + +void OdeWorld::Object::loadPhy(const std::string & path) +{ ifstream ifs(path.c_str()); if (ifs.is_open()) { @@ -123,87 +188,76 @@ vector OdeWorld::loadPhy(const std::string & path, } if (type == "cube") { - ret.push_back(addCube(name, is_static, body, args)); + addCube(args); } else if (type == "sphere") { - ret.push_back(addSphere(name, is_static, body, args)); + addSphere(args); } else if (type == "cylinder") { - ret.push_back(addCylinder(name, is_static, body, args)); + addCylinder(args); } else if (type == "plane") { - ret.push_back(addPlane(name, is_static, body, args)); + addPlane(args); } } } - - return ret; } -dGeomID OdeWorld::addCube(const string & name, bool is_static, - dBodyID * body, const vector args) +void OdeWorld::Object::addCube(const vector args) { if (args.size() != 9) - return 0; + return; dGeomID id = dCreateBox(0, args[0], args[1], args[2]); dMass mass; dMassSetBox(&mass, 1.0, args[0], args[1], args[2]); - setupGeom(name, is_static, body, id, &mass, + setupGeom(id, &mass, args[3], args[4], args[5], args[6], args[7], args[8]); - return id; } -dGeomID OdeWorld::addSphere(const string & name, bool is_static, - dBodyID * body, const vector args) +void OdeWorld::Object::addSphere(const vector args) { if (args.size() != 4) - return 0; + return; dGeomID id = dCreateSphere(0, args[0]); dMass mass; dMassSetSphere(&mass, 1.0, args[0]); - setupGeom(name, is_static, body, id, &mass, + setupGeom(id, &mass, args[1], args[2], args[3], 0.0, 0.0, 0.0); - return id; } -dGeomID OdeWorld::addCylinder(const string & name, bool is_static, - dBodyID * body, const vector args) +void OdeWorld::Object::addCylinder(const vector args) { if (args.size() != 8) - return 0; + return; dGeomID id = dCreateCylinder(0, args[0], args[1]); dMass mass; dMassSetCylinder(&mass, 1.0, 3, args[0], args[1]); - setupGeom(name, is_static, body, id, &mass, + setupGeom(id, &mass, args[2], args[3], args[4], args[5], args[6], args[7]); - return id; } -dGeomID OdeWorld::addCCylinder(const string & name, bool is_static, - dBodyID * body, const vector args) +void OdeWorld::Object::addCCylinder(const vector args) { if (args.size() != 8) - return 0; + return; dGeomID id = dCreateCCylinder(0, args[0], args[1]); dMass mass; dMassSetCappedCylinder(&mass, 1.0, 3, args[0], args[1]); - setupGeom(name, is_static, body, id, &mass, + setupGeom(id, &mass, args[2], args[3], args[4], args[5], args[6], args[7]); - return id; } -dGeomID OdeWorld::addPlane(const string & name, bool is_static, - dBodyID * body, const vector args) +void OdeWorld::Object::addPlane(const vector args) { if (args.size() != 6) - return 0; + return; dMatrix3 r; dRFromEulerAngles(r, args[3], args[4], args[5]); @@ -217,11 +271,10 @@ dGeomID OdeWorld::addPlane(const string & name, bool is_static, float d = a * args[0] + b * args[1] + c * args[2]; dGeomID id = dCreatePlane(m_space, a, b, c, d); - return id; + m_geoms.push_back(id); } -void OdeWorld::setupGeom(const std::string & name, bool is_static, - dBodyID * body, dGeomID geom, dMass * mass, +void OdeWorld::Object::setupGeom(dGeomID geom, dMass * mass, float locx, float locy, float locz, float rotx, float roty, float rotz) { @@ -230,89 +283,26 @@ void OdeWorld::setupGeom(const std::string & name, bool is_static, dGeomSetRotation(geom, rot); dGeomSetPosition(geom, locx, locy, locz); - if (!is_static) + dGeomID transform = dCreateGeomTransform(m_space); + dGeomTransformSetCleanup(transform, 1); + dGeomTransformSetInfo(transform, 1); + dGeomTransformSetGeom(transform, geom); + + m_geoms.push_back(transform); + + if (!m_is_static) { /* attach the geometry to the body */ - if (*body == 0) - *body = dBodyCreate(m_world); + if (m_body == 0) + m_body = dBodyCreate(m_world); - dGeomId transform = dCreateGeomTransform(m_space); - dGeomTransformSetGeom(transform, geom); - dGeomTransformSetCleanup(transform, 1); - - dGeomSetBody(transform, *body); + dGeomSetBody(transform, m_body); dMassRotate(mass, rot); dMassTranslate(mass, locx, locy, locz); dMass origmass; - dBodyGetMass(*body, &origmass); + dBodyGetMass(m_body, &origmass); dMassAdd(&origmass, mass); - dBodySetMass(*body, &origmass); - } -} - -/* push an OpenGL matrix onto the matrix stack for a given - * ODE body position and rotation */ -void OdeWorld::pushTransform(const float pos[3], const float R[12]) -{ - GLfloat matrix[16]; - matrix[0] = R[0]; - matrix[1] = R[4]; - matrix[2] = R[8]; - matrix[3] = 0; - matrix[4] = R[1]; - matrix[5] = R[5]; - matrix[6] = R[9]; - matrix[7] = 0; - matrix[8] = R[2]; - matrix[9] = R[6]; - matrix[10] = R[10]; - matrix[11] = 0; - matrix[12] = pos[0]; - matrix[13] = pos[1]; - matrix[14] = pos[2]; - matrix[15] = 1; - glPushMatrix(); - glMultMatrixf(matrix); -} - -OdeWorld::Object::Object(bool is_static) -{ - m_is_static = is_static; -} - -void OdeWorld::Object::setPosition(double x, double y, double z) -{ - if (m_is_static) - { - for (int i = 0, sz = m_geoms.size(); i < sz; i++) - dGeomSetPosition(m_geoms[i], x, y, z); - } - else - { - if (m_body != 0) - dBodySetPosition(m_body, x, y, z); - } -} - -void OdeWorld::Object::getPosition(double * x, double * y, double * z) -{ - const dReal * pos = NULL; - if (m_is_static) - { - if (m_geoms.size() > 0) - pos = dGeomGetPosition(m_geoms[0]); - } - else - { - if (m_body != 0) - pos = dBodyGetPosition(body); - } - - if (pos != NULL) - { - *x = pos[0]; - *y = pos[1]; - *z = pos[2]; + dBodySetMass(m_body, &origmass); } } diff --git a/OdeWorld.h b/OdeWorld.h index 63b7007..60b6773 100644 --- a/OdeWorld.h +++ b/OdeWorld.h @@ -16,16 +16,30 @@ class OdeWorld class Object { public: - Object(bool is_static); + Object(bool is_static, dWorldID world, dSpaceID space); void loadPhy(const std::string & path); void setPosition(double x, double y, double z); void getPosition(double * x, double * y, double * z); + 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); protected: bool m_is_static; dBodyID m_body; + dWorldID m_world; + dSpaceID m_space; std::vector m_geoms; + + void setupGeom(dGeomID geom, dMass * mass, + float locx, float locy, float locz, + float rotx, float roty, float rotz); + }; void setGravity(float x, float y, float z) @@ -33,18 +47,6 @@ class OdeWorld dWorldSetGravity(m_world, x, y, z); } void step(); - std::vector loadPhy(const std::string & path, - dBodyID * body, bool is_static = false); - dGeomID addCube(const std::string & name, bool is_static, - dBodyID * body, const std::vector args); - dGeomID addSphere(const std::string & name, bool is_static, - dBodyID * body, const std::vector args); - dGeomID addCylinder(const std::string & name, bool is_static, - dBodyID * body, const std::vector args); - dGeomID addCCylinder(const std::string & name, bool is_static, - dBodyID * body, const std::vector args); - dGeomID addPlane(const std::string & name, bool is_static, - dBodyID * body, const std::vector args); friend void OdeWorld_collide_callback(void * data, dGeomID o1, dGeomID o2); @@ -56,11 +58,6 @@ class OdeWorld dSpaceID m_space; dJointGroupID m_contactJointGroup; std::map m_objects; - - void setupGeom(const std::string & name, bool is_static, - dBodyID * body, dGeomID geom, dMass * mass, - float locx, float locy, float locz, - float rotx, float roty, float rotz); }; #endif