diff --git a/OdeWorld.cc b/OdeWorld.cc index 1b06556..9e4f3c6 100644 --- a/OdeWorld.cc +++ b/OdeWorld.cc @@ -63,7 +63,7 @@ void OdeWorld::step() OdeWorld::Object * OdeWorld::createObject(bool is_static, float scale) { - return new Object(is_static, m_world, m_space, scale); + return new Object(is_static, this, m_world, m_space, scale); } /* push an OpenGL matrix onto the matrix stack for a given @@ -91,7 +91,27 @@ void OdeWorld::pushTransform(const dReal * pos, const dReal * R) glMultMatrixf(matrix); } -OdeWorld::Object::Object(bool is_static, dWorldID world, dSpaceID space, +dBodyID OdeWorld::createBody() +{ + dBodyID id = dBodyCreate(m_world); + m_bodies[id] = 1; + return id; +} + +void OdeWorld::destroyBody(dBodyID body) +{ + m_bodies.erase(body); + dBodyDestroy(body); + for (std::map::iterator it = m_bodies.begin(); + it != m_bodies.end(); + it++) + { + dBodyEnable(it->first); + } +} + +OdeWorld::Object::Object(bool is_static, OdeWorld * ode_world, + dWorldID world, dSpaceID space, float scale) { m_is_static = is_static; @@ -99,6 +119,7 @@ OdeWorld::Object::Object(bool is_static, dWorldID world, dSpaceID space, m_space = space; m_body = 0; m_scale = scale; + m_ode_world = ode_world; dMassSetZero(&m_mass); for (int i = 0; i < 3; i++) m_position[i] = 0.0; @@ -113,6 +134,7 @@ OdeWorld::Object::Object(const OdeWorld::Object & orig) m_world = orig.m_world; m_space = orig.m_space; m_scale = orig.m_scale; + m_ode_world = orig.m_ode_world; for (int i = 0; i < 3; i++) m_position[i] = orig.m_position[i]; for (int i = 0; i < 12; i++) @@ -120,7 +142,7 @@ OdeWorld::Object::Object(const OdeWorld::Object & orig) /* make a copy of the ODE body */ if (orig.m_body != 0) { - m_body = dBodyCreate(m_world); + m_body = m_ode_world->createBody(); m_mass = orig.m_mass; dBodySetMass(m_body, &m_mass); const dReal * pos = dBodyGetPosition(orig.m_body); @@ -166,7 +188,7 @@ OdeWorld::Object::~Object() for (int i = 0, sz = m_geoms.size(); i < sz; i++) dGeomDestroy(m_geoms[i]); if (m_body != 0) - dBodyDestroy(m_body); + m_ode_world->destroyBody(m_body); } dGeomID OdeWorld::Object::cloneGeom(dGeomID geom, dBodyID body) @@ -366,7 +388,7 @@ void OdeWorld::Object::setupGeom(dGeomID geom, dMass * mass, { /* attach the geometry to the body */ if (m_body == 0) - m_body = dBodyCreate(m_world); + m_body = m_ode_world->createBody(); dGeomSetBody(transform, m_body); diff --git a/OdeWorld.h b/OdeWorld.h index 69a7f36..7590d8d 100644 --- a/OdeWorld.h +++ b/OdeWorld.h @@ -19,7 +19,8 @@ class OdeWorld class Object { public: - Object(bool is_static, dWorldID world, dSpaceID space, + Object(bool is_static, OdeWorld * ode_world, + dWorldID world, dSpaceID space, float scale = 1.0f); Object(const Object & orig); ~Object(); @@ -50,6 +51,7 @@ class OdeWorld float m_scale; dReal m_position[3]; dReal m_rotation[12]; + OdeWorld * m_ode_world; void setupGeom(dGeomID geom, dMass * mass, float locx, float locy, float locz, @@ -70,9 +72,13 @@ class OdeWorld static void pushTransform(const dReal * pos, const dReal * R); protected: + dBodyID createBody(); + void destroyBody(dBodyID body); + dWorldID m_world; dSpaceID m_space; dJointGroupID m_contactJointGroup; + std::map m_bodies; }; #endif