diff --git a/OdeWorld.cc b/OdeWorld.cc index 0e62880..f567618 100644 --- a/OdeWorld.cc +++ b/OdeWorld.cc @@ -116,6 +116,7 @@ OdeWorld::Object::Object(bool is_static, dWorldID world, dSpaceID space) m_world = world; m_space = space; m_body = 0; + dMassSetZero(&m_mass); } OdeWorld::Object::Object(const OdeWorld::Object & orig) @@ -127,9 +128,8 @@ OdeWorld::Object::Object(const OdeWorld::Object & orig) if (orig.m_body != 0) { m_body = dBodyCreate(m_world); - dMass mass; - dBodyGetMass(orig.m_body, &mass); - dBodySetMass(m_body, &mass); + m_mass = orig.m_mass; + dBodySetMass(m_body, &m_mass); const dReal * pos = dBodyGetPosition(orig.m_body); dBodySetPosition(m_body, pos[0], pos[1], pos[2]); const dReal * rot = dBodyGetRotation(orig.m_body); @@ -340,6 +340,11 @@ void OdeWorld::Object::loadPhy(const std::string & path) } } } + 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) @@ -436,10 +441,7 @@ void OdeWorld::Object::setupGeom(dGeomID geom, dMass * mass, dMassRotate(mass, rot); dMassTranslate(mass, locx, locy, locz); - dMass origmass; - dBodyGetMass(m_body, &origmass); - dMassAdd(&origmass, mass); - dBodySetMass(m_body, &origmass); + dMassAdd(&m_mass, mass); } } diff --git a/OdeWorld.h b/OdeWorld.h index 43b3f7a..0c10d59 100644 --- a/OdeWorld.h +++ b/OdeWorld.h @@ -35,6 +35,7 @@ class OdeWorld protected: bool m_is_static; dBodyID m_body; + dMass m_mass; dWorldID m_world; dSpaceID m_space; std::vector m_geoms;