// converts a osg 4x4 matrix to an ode version of it
void odeRotation( const Pose& pose , dMatrix3& odematrix){
osg::Quat q;
pose.get(q);
// THIS should be
// dQuaternion quat = {q.x(), q.y(), q.z(), q.w() };
dQuaternion quat = {q.w(), q.x(), q.y(), q.z() };
dQtoR(quat, odematrix);
}
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
}
