Better update of ghost object pose

WPI-AIM · Jul 2, 2024 · fd5520b · fd5520b
1 parent 137a3aa
commit fd5520b
Showing 1 changed file with 12 additions and 1 deletion.
diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp
@@ -8030,7 +8030,18 @@ void afGhostObject::update(double dt)
     //    cTransform trans;
     //    trans << m_bulletGhostObject->getWorldTransform();
     //    setLocalTransform(trans);
-    m_bulletGhostObject->setWorldTransform(to_btTransform(m_globalTransform));
+    cTransform T_g_p = getLocalTransform();
+    if (m_parentObject){
+        cTransform T_p_w;
+        if (m_parentObject->getType() == afType::RIGID_BODY || m_parentObject->getType() == afType::SOFT_BODY || m_parentObject->getType() == afType::GHOST_OBJECT){
+            T_p_w << ((afInertialObject*)m_parentObject)->getCOMTransform();
+        }
+        else{
+            T_p_w = m_parentObject->getGlobalTransform();
+        }
+        T_g_p = T_p_w * T_g_p;
+    }
+    m_bulletGhostObject->setWorldTransform(to_btTransform(T_g_p));
     m_sensedObjectsMaps.clear();
 
     btManifoldArray* manifoldArray = new btManifoldArray();