Fixed setPose for dynamics objects

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@565 042f3d55-54a8-47e9-b7fb-15903f145c44

SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp

@@ -181,8 +181,14 @@ void BulletObject::setPosition( const Eigen::Vector3f &posMM )
 
 void BulletObject::setPoseIntern( const Eigen::Matrix4f &pose )
 {
-    // notify motionState
-    motionState->setGlobalPose(pose);
+    /* convert to local coord system, apply comoffset and convert back*/
+    Eigen::Matrix4f poseLocal = sceneObject->getGlobalPose().inverse() * pose;
+    poseLocal.block(0, 3, 3, 1) += com;
+    Eigen::Matrix4f poseGlobal = sceneObject->getGlobalPose() * poseLocal;
+    this->rigidBody->setWorldTransform(BulletEngine::getPoseBullet(poseGlobal));
+
+    // notify motionState -> not needed, automatically done
+    //motionState->setGlobalPose(pose);
 }
 
 void BulletObject::setPose( const Eigen::Matrix4f &pose )

SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp

@@ -63,7 +63,7 @@ SimDynamicsWindow::SimDynamicsWindow(std::string &sRobotFilename, Qt::WFlags fla
     o->setMass(1.0f); // 1kg
 
 	dynamicsObject = dynamicsWorld->CreateDynamicsObject(o);
-    dynamicsObject->setPosition(Eigen::Vector3f(3000,3000,10000.0f));
+    dynamicsObject->setPosition(Eigen::Vector3f(1000,2000,1000.0f));
 	dynamicsWorld->addObject(dynamicsObject);
 
 #if 0