void Mesh::setPose(const Pose& pose){ if(body){ osg::Vec3 pos = pose.getTrans(); dBodySetPosition(body, pos.x(), pos.y(), pos.z()); osg::Quat q; pose.get(q); // this should be // dReal quat[4] = {q.x(), q.y(), q.z(), q.w()}; dReal quat[4] = {q.w(), q.x(), q.y(), q.z()}; dBodySetQuaternion(body, quat); }else if(geom){ // okay there is just a geom no body osg::Vec3 pos = pose.getTrans(); dGeomSetPosition(geom, pos.x(), pos.y(), pos.z()); osg::Quat q; pose.get(q); // this should be // dReal quat[4] = {q.x(), q.y(), q.z(), q.w()}; dReal quat[4] = {q.w(), q.x(), q.y(), q.z()}; dGeomSetQuaternion(geom, quat); } else poseWithoutBodyAndGeom = Pose(pose); update(); // update the scenegraph stuff }
void Primitive::setPose(const Pose& pose){ if(body){ osg::Vec3 pos = pose.getTrans(); dBodySetPosition(body, pos.x(), pos.y(), pos.z()); osg::Quat q; pose.get(q); // this should be // dReal quat[4] = {q.x(), q.y(), q.z(), q.w()}; dReal quat[4] = {q.w(), q.x(), q.y(), q.z()}; dBodySetQuaternion(body, quat); }else if(geom){ // okay there is just a geom no body osg::Vec3 pos = pose.getTrans(); dGeomSetPosition(geom, pos.x(), pos.y(), pos.z()); osg::Quat q; pose.get(q); // this should be // dReal quat[4] = {q.x(), q.y(), q.z(), q.w()}; dReal quat[4] = {q.w(), q.x(), q.y(), q.z()}; dGeomSetQuaternion(geom, quat); } else { assert(0 && "Call setPose only after initialization"); } update(); // update the scenegraph stuff }