diff --git a/OdeWorld.cc b/OdeWorld.cc index fe0c5a6..0e62880 100644 --- a/OdeWorld.cc +++ b/OdeWorld.cc @@ -134,6 +134,28 @@ OdeWorld::Object::Object(const OdeWorld::Object & orig) dBodySetPosition(m_body, pos[0], pos[1], pos[2]); const dReal * rot = dBodyGetRotation(orig.m_body); dBodySetRotation(m_body, rot); + dBodySetAutoDisableFlag(m_body, + dBodyGetAutoDisableFlag(orig.m_body)); + dBodySetAutoDisableLinearThreshold(m_body, + dBodyGetAutoDisableLinearThreshold(orig.m_body)); + dBodySetAutoDisableAngularThreshold(m_body, + dBodyGetAutoDisableAngularThreshold(orig.m_body)); + dBodySetAutoDisableSteps(m_body, + dBodyGetAutoDisableSteps(orig.m_body)); + dBodySetAutoDisableTime(m_body, + dBodyGetAutoDisableTime(orig.m_body)); + int finite_rotation_mode = dBodyGetFiniteRotationMode(orig.m_body); + dBodySetFiniteRotationMode(m_body, finite_rotation_mode); + if (finite_rotation_mode) + { + dVector3 finite_rotation_axis; + dBodyGetFiniteRotationAxis(orig.m_body, finite_rotation_axis); + dBodySetFiniteRotationAxis(m_body, + finite_rotation_axis[0], + finite_rotation_axis[1], + finite_rotation_axis[2]); + } + dBodySetGravityMode(m_body, dBodyGetGravityMode(orig.m_body)); } else { @@ -148,20 +170,55 @@ OdeWorld::Object::Object(const OdeWorld::Object & orig) dGeomID OdeWorld::Object::cloneGeom(dGeomID geom, dBodyID body) { - /* TODO: clone the geom */ + dGeomID id = 0; + dSpaceID space = dGeomGetSpace(geom); switch (dGeomGetClass(geom)) { case dSphereClass: + id = dCreateSphere(space, dGeomSphereGetRadius(geom)); break; case dBoxClass: + { + dVector3 size; + dGeomBoxGetLengths(geom, size); + id = dCreateBox(space, size[0], size[1], size[2]); + } break; case dCCylinderClass: + { + dReal radius, length; + dGeomCylinderGetParams(geom, &radius, &length); + id = dCreateCylinder(space, radius, length); + } break; case dCylinderClass: + { + dReal radius, length; + dGeomCCylinderGetParams(geom, &radius, &length); + id = dCreateCCylinder(space, radius, length); + } break; case dPlaneClass: + { + dVector4 params; + dGeomPlaneGetParams(geom, params); + id = dCreatePlane(space, + params[0], params[1], params[2], params[3]); + } break; case dGeomTransformClass: + { + dGeomID old_inner_geom = dGeomTransformGetGeom(geom); + dGeomID new_inner_geom = cloneGeom(old_inner_geom, body); + if (new_inner_geom != 0) + { + id = dCreateGeomTransform(space); + dGeomTransformSetGeom(id, new_inner_geom); + dGeomTransformSetCleanup(id, + dGeomTransformGetCleanup(geom)); + dGeomTransformSetInfo(id, dGeomTransformGetInfo(geom)); + } + } break; case dRayClass: case dTriMeshClass: @@ -170,6 +227,25 @@ dGeomID OdeWorld::Object::cloneGeom(dGeomID geom, dBodyID body) /* unsupported for cloning */ break; } + if (id != 0) + { + if (dGeomGetBody(geom) != 0) + { + /* if the original geom was attached to a body + * (i.e., a non-static geom), then attach it to the new body */ + dGeomSetBody(id, body); + } + else if (dGeomGetClass(id) != dPlaneClass) + { + /* if the original geom was static, then copy the + * position and rotation to the new geom */ + const dReal * pos = dGeomGetPosition(geom); + const dReal * rot = dGeomGetRotation(geom); + dGeomSetPosition(id, pos[0], pos[1], pos[2]); + dGeomSetRotation(id, rot); + } + } + return id; } void OdeWorld::Object::setPosition(double x, double y, double z)