add multibody interpolation transform so that collision detection is consistent with rigidbody

10 files changed, 196 insertions, 46 deletions

diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index 3e210d752..80ad19891 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -27,6 +27,7 @@
 #include "btMultiBodyJointFeedback.h"
 #include "LinearMath/btTransformUtil.h"
 #include "LinearMath/btSerializer.h"
+#include <iostream>
 //#include "Bullet3Common/b3Logging.h"
 // #define INCLUDE_GYRO_TERM
 
@@ -100,6 +101,8 @@ btMultiBody::btMultiBody(int n_links,
 	  m_baseName(0),
 	  m_basePos(0, 0, 0),
 	  m_baseQuat(0, 0, 0, 1),
+      m_basePos_interpolate(0, 0, 0),
+      m_baseQuat_interpolate(0, 0, 0, 1),
 	  m_baseMass(mass),
 	  m_baseInertia(inertia),
 
@@ -449,6 +452,16 @@ const btQuaternion &btMultiBody::getParentToLocalRot(int i) const
 	return m_links[i].m_cachedRotParentToThis;
 }
 
+const btVector3 &btMultiBody::getInterpolateRVector(int i) const
+{
+    return m_links[i].m_cachedRVector_interpolate;
+}
+
+const btQuaternion &btMultiBody::getInterpolateParentToLocalRot(int i) const
+{
+    return m_links[i].m_cachedRotParentToThis_interpolate;
+}
+
 btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
 {
 	btAssert(i >= -1);
@@ -1581,17 +1594,37 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	//printf("]\n");
 	/////////////////
 }
+void btMultiBody::predictPositionsMultiDof(btScalar dt)
+{
+    stepPositionsMultiDof(dt, 0, 0, true);
+}
 
-void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
+void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd, bool predict)
 {
 	int num_links = getNumLinks();
 	// step position by adding dt * velocity
 	//btVector3 v = getBaseVel();
 	//m_basePos += dt * v;
 	//
-	btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
-	btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
-	//
+    btScalar *pBasePos;
+    btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
+    
+    if (!predict)
+    {
+	    pBasePos = (pq ? &pq[4] : m_basePos);
+    }	//
+    else
+    {
+        // reset to current position
+        for (int i = 0; i < 3; ++i)
+        {
+            m_basePos_interpolate[i] = m_basePos[i];
+        }
+        pBasePos = m_basePos_interpolate;
+    }
+    
+    
+    
 	pBasePos[0] += dt * pBaseVel[0];
 	pBasePos[1] += dt * pBaseVel[1];
 	pBasePos[2] += dt * pBaseVel[2];
@@ -1645,7 +1678,18 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 
 	//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
 	//
-	btScalar *pBaseQuat = pq ? pq : m_baseQuat;
+    btScalar *pBaseQuat;
+    if (!predict)
+        pBaseQuat = pq ? pq : m_baseQuat;
+    else
+    {
+        // reset to current orientation
+        for (int i = 0; i < 4; ++i)
+        {
+            m_baseQuat_interpolate[i] = m_baseQuat[i];
+        }
+        pBaseQuat = m_baseQuat_interpolate;
+    }
 	btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
 	//
 	btQuaternion baseQuat;
@@ -1670,7 +1714,12 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 	// Finally we can update m_jointPos for each of the m_links
 	for (int i = 0; i < num_links; ++i)
 	{
-		btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);
+        btScalar *pJointPos;
+        if (!predict)
+            pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
+        else
+            pJointPos = &m_links[i].m_jointPos_interpolate[0];
+        
 		btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
 
 		switch (m_links[i].m_jointType)
@@ -1678,12 +1727,23 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 			case btMultibodyLink::ePrismatic:
 			case btMultibodyLink::eRevolute:
 			{
+                //reset to current pos
+                if (predict)
+                {
+                    pJointPos[0] = m_links[i].m_jointPos[0];
+                }
 				btScalar jointVel = pJointVel[0];
 				pJointPos[0] += dt * jointVel;
 				break;
 			}
 			case btMultibodyLink::eSpherical:
 			{
+                //reset to current pos
+                if (predict)
+                {
+                    for (int i = 0; i < 4; ++i)
+                        pJointPos[i] = m_links[i].m_jointPos[i];
+                }
 				btVector3 jointVel;
 				jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
 				btQuaternion jointOri;
@@ -1697,6 +1757,11 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 			}
 			case btMultibodyLink::ePlanar:
 			{
+                if (predict)
+                {
+                    for (int i = 0; i < 3; ++i)
+                        pJointPos[i] = m_links[i].m_jointPos[i];
+                }
 				pJointPos[0] += dt * getJointVelMultiDof(i)[0];
 
 				btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
@@ -1711,7 +1776,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 			}
 		}
 
-		m_links[i].updateCacheMultiDof(pq);
+		m_links[i].updateCacheMultiDof(pq, predict);
 
 		if (pq)
 			pq += m_links[i].m_posVarCount;
@@ -2006,6 +2071,57 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQu
 	}
 }
 
+void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
+{
+    world_to_local.resize(getNumLinks() + 1);
+    local_origin.resize(getNumLinks() + 1);
+    
+    world_to_local[0] = getInterpolateWorldToBaseRot();
+    local_origin[0] = getInterpolateBasePos();
+    
+    if (getBaseCollider())
+    {
+        btVector3 posr = local_origin[0];
+        //    float pos[4]={posr.x(),posr.y(),posr.z(),1};
+        btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
+        btTransform tr;
+        tr.setIdentity();
+        tr.setOrigin(posr);
+        tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+        
+        getBaseCollider()->setInterpolationWorldTransform(tr);
+    }
+    
+    for (int k = 0; k < getNumLinks(); k++)
+    {
+        const int parent = getParent(k);
+        world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1];
+        local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k)));
+    }
+    
+    for (int m = 0; m < getNumLinks(); m++)
+    {
+        btMultiBodyLinkCollider *col = getLink(m).m_collider;
+        if (col)
+        {
+            int link = col->m_link;
+            btAssert(link == m);
+            
+            int index = link + 1;
+            
+            btVector3 posr = local_origin[index];
+            //            float pos[4]={posr.x(),posr.y(),posr.z(),1};
+            btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
+            btTransform tr;
+            tr.setIdentity();
+            tr.setOrigin(posr);
+            tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+            
+            col->setInterpolationWorldTransform(tr);
+        }
+    }
+}
+
 int btMultiBody::calculateSerializeBufferSize() const
 {
 	int sz = sizeof(btMultiBodyData);
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h
index c0b0d003b..fe60af991 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -193,12 +193,24 @@ public:
 	const btQuaternion &getWorldToBaseRot() const
 	{
 		return m_baseQuat;
-	}                                                                                               // rotates world vectors into base frame
+	}
+    
+    const btVector3 &getInterpolateBasePos() const
+    {
+        return m_basePos_interpolate;
+    }  // in world frame
+    const btQuaternion &getInterpolateWorldToBaseRot() const
+    {
+        return m_baseQuat_interpolate;
+    }
+    
+    // rotates world vectors into base frame
 	btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); }  // in world frame
 
 	void setBasePos(const btVector3 &pos)
 	{
 		m_basePos = pos;
+        m_basePos_interpolate = pos;
 	}
 
 	void setBaseWorldTransform(const btTransform &tr)
@@ -224,6 +236,7 @@ public:
 	void setWorldToBaseRot(const btQuaternion &rot)
 	{
 		m_baseQuat = rot;  //m_baseQuat asumed to ba alias!?
+        m_baseQuat_interpolate = rot;
 	}
 	void setBaseOmega(const btVector3 &omega)
 	{
@@ -273,6 +286,8 @@ public:
 
 	const btVector3 &getRVector(int i) const;              // vector from COM(parent(i)) to COM(i), in frame i's coords
 	const btQuaternion &getParentToLocalRot(int i) const;  // rotates vectors in frame parent(i) to vectors in frame i.
+    const btVector3 &getInterpolateRVector(int i) const;              // vector from COM(parent(i)) to COM(i), in frame i's coords
+    const btQuaternion &getInterpolateParentToLocalRot(int i) const;  // rotates vectors in frame parent(i) to vectors in frame i.
 
 	//
 	// transform vectors in local frame of link i to world frame (or vice versa)
@@ -420,7 +435,10 @@ public:
 	}
 
 	// timestep the positions (given current velocities).
-	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
+	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0, bool predict = false);
+    
+    // predict the positions
+    void predictPositionsMultiDof(btScalar dt);
 
 	//
 	// contacts
@@ -581,6 +599,7 @@ public:
 	void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const;
 
 	void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
+    void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
 
 	virtual int calculateSerializeBufferSize() const;
 
@@ -664,7 +683,9 @@ private:
 	const char *m_baseName;                   //memory needs to be manager by user!
 
 	btVector3 m_basePos;      // position of COM of base (world frame)
+    btVector3 m_basePos_interpolate;      // position of interpolated COM of base (world frame)
 	btQuaternion m_baseQuat;  // rotates world points into base frame
+    btQuaternion m_baseQuat_interpolate;  
 
 	btScalar m_baseMass;      // mass of the base
 	btVector3 m_baseInertia;  // inertia of the base (in local frame; diagonal)
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index d4a9a754f..d1ac5e26f 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -33,6 +33,12 @@ void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
 	m_multiBodies.remove(body);
 }
 
+void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+    btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
+    integrateMultiBodyTransforms(timeStep, /*predict = */ true);
+    
+}
 void btMultiBodyDynamicsWorld::calculateSimulationIslands()
 {
 	BT_PROFILE("calculateSimulationIslands");
@@ -778,8 +784,11 @@ void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverIn
 void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
 	btDiscreteDynamicsWorld::integrateTransforms(timeStep);
+    integrateMultiBodyTransforms(timeStep);
+}
 
-	{
+void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, bool predict)
+{
 		BT_PROFILE("btMultiBody stepPositions");
 		//integrate and update the Featherstone hierarchies
 
@@ -802,7 +811,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 				int nLinks = bod->getNumLinks();
 
 				///base + num m_links
-
+                if (!predict)
 				{
 					if (!bod->isPosUpdated())
 						bod->stepPositionsMultiDof(timeStep);
@@ -815,18 +824,21 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 						bod->setPosUpdated(false);
 					}
 				}
+                else
+                    bod->predictPositionsMultiDof(timeStep);
 
 				m_scratch_world_to_local.resize(nLinks + 1);
 				m_scratch_local_origin.resize(nLinks + 1);
-
-				bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+                if (predict)
+                    bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+                else
+                    bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
 			}
 			else
 			{
 				bod->clearVelocities();
 			}
 		}
-	}
 }
 
 void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint)
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
index 4f48f07d2..e82c17b31 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
@@ -96,7 +96,9 @@ public:
 	virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint);
 
 	virtual void integrateTransforms(btScalar timeStep);
-
+    void integrateMultiBodyTransforms(btScalar timeStep,bool predict = false);
+    
+    virtual void predictUnconstraintMotion(btScalar timeStep);
 	virtual void debugDrawWorld();
 
 	virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
index 92d41dfac..e7966b852 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
@@ -111,6 +111,10 @@ struct btMultibodyLink
 
 	btQuaternion m_cachedRotParentToThis;  // rotates vectors in parent frame to vectors in local frame
 	btVector3 m_cachedRVector;             // vector from COM of parent to COM of this link, in local frame.
+    
+    // predicted verstion
+    btQuaternion m_cachedRotParentToThis_interpolate;  // rotates vectors in parent frame to vectors in local frame
+    btVector3 m_cachedRVector_interpolate;             // vector from COM of parent to COM of this link, in local frame.
 
 	btVector3 m_appliedForce;   // In WORLD frame
 	btVector3 m_appliedTorque;  // In WORLD frame
@@ -119,6 +123,7 @@ struct btMultibodyLink
 	btVector3 m_appliedConstraintTorque;  // In WORLD frame
 
 	btScalar m_jointPos[7];
+    btScalar m_jointPos_interpolate[7];
 
 	//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
 	//It gets set to zero after each internal stepSimulation call
@@ -186,44 +191,50 @@ struct btMultibodyLink
 	}
 
 	// routine to update m_cachedRotParentToThis and m_cachedRVector
-	void updateCacheMultiDof(btScalar *pq = 0)
+	void updateCacheMultiDof(btScalar *pq = 0, bool predict = false)
 	{
-		btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
-
+        btScalar *pJointPos;
+        
+        if (!predict)
+            pJointPos = (pq ? pq : &m_jointPos[0]);
+        else
+            pJointPos = &m_jointPos_interpolate[0];
+        btQuaternion& cachedRot = predict ? m_cachedRotParentToThis_interpolate : m_cachedRotParentToThis;
+        btVector3& cachedVector = predict ? m_cachedRVector_interpolate : m_cachedRVector;
 		switch (m_jointType)
 		{
 			case eRevolute:
 			{
-				m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+				cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
 
 				break;
 			}
 			case ePrismatic:
 			{
 				// m_cachedRotParentToThis never changes, so no need to update
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
+				cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
 
 				break;
 			}
 			case eSpherical:
 			{
-				m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+				cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
 			case ePlanar:
 			{
-				m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
-				m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis, m_eVector);
+				cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+				cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
 			case eFixed:
 			{
-				m_cachedRotParentToThis = m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+				cachedRot = m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
index acc9db75f..9ffd63a3c 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
@@ -12,12 +12,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned
 , projection(m_softBodies, m_dt, &m_indices)
 , m_backupVelocity(backup_v)
 {
-    // TODO: this should really be specified in initialization instead of here
-//    btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(m_softBodies);
-//    btDeformableGravityForce* gravity = new btDeformableGravityForce(m_softBodies, btVector3(0,-10,0));
     m_preconditioner = new DefaultPreconditioner();
-//    m_lf.push_back(mass_spring);
-//    m_lf.push_back(gravity);
 }
 
 void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated)
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
index 4d6ea4e84..734fbb451 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.cpp
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -44,7 +44,7 @@ static btVector3 generateUnitOrthogonalVector(const btVector3& u)
 void btDeformableContactProjection::update()
 {
     ///solve rigid body constraints
-    m_world->getSolverInfo().m_numIterations = 10;
+    m_world->getSolverInfo().m_numIterations = 1;
     m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo());
 
     // loop through constraints to set constrained values
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
index b91d9d2c4..6ff5a9a41 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
@@ -161,7 +161,7 @@ void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collision
 
 void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
-    btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
+    btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep);
     m_deformableBodySolver->predictMotion(float(timeStep));
 }
 
diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index c31bdb7ed..06e0b41d0 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -2270,12 +2270,10 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
 {
 	btVector3 nrm;
 	const btCollisionShape* shp = colObjWrap->getCollisionShape();
-    const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
-    
+    const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
     // get the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg
-    const btTransform &wtr = (tmpRigid&&predict) ? tmpRigid->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
-//    const btTransform &wtr = predict ? colObjWrap->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
-    // TODO: get the correct transform for multibody
+    const btTransform &wtr = (predict) ? tmpCollisionObj->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
+//    const btTransform &wtr = colObjWrap->getWorldTransform();
 
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index 2ec8c1b55..34302ccc4 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -985,13 +985,8 @@ struct btSoftColliders
                                 btVector3 t2 = btCross(normal, t1);
                                 btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
                                 findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_q, normal);
-                                
-                                // findJacobian is hella expensive, avoid calling if possible
-                                if (fc != 0)
-                                {
-                                    findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1);
-                                    findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2);
-                                }
+                                findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1);
+                                findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2);
                                 
                                 btScalar* J_n = &jacobianData_normal.m_jacobians[0];
                                 btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];