separate multibody position prediction into standalone function

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -1594,11 +1594,6 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	/////////////////
 }
 void btMultiBody::predictPositionsMultiDof(btScalar dt)
-{
-    stepPositionsMultiDof(dt, 0, 0, true);
-}
-
-void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd, bool predict)
 {
     int num_links = getNumLinks();
     // step position by adding dt * velocity
@@ -1606,23 +1601,14 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
     //m_basePos += dt * v;
     //
     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)
+    btScalar *pBaseVel = &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];
@@ -1678,17 +1664,152 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
     //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
     //
     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 = &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
+    //
+    btQuaternion baseQuat;
+    baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+    btVector3 baseOmega;
+    baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+    pQuatUpdateFun(baseOmega, baseQuat, true, dt);
+    pBaseQuat[0] = baseQuat.x();
+    pBaseQuat[1] = baseQuat.y();
+    pBaseQuat[2] = baseQuat.z();
+    pBaseQuat[3] = baseQuat.w();
+
+    // Finally we can update m_jointPos for each of the m_links
+    for (int i = 0; i < num_links; ++i)
+    {
+        btScalar *pJointPos;
+        pJointPos = &m_links[i].m_jointPos_interpolate[0];
+        
+        btScalar *pJointVel = getJointVelMultiDof(i);
+        
+        switch (m_links[i].m_jointType)
+        {
+            case btMultibodyLink::ePrismatic:
+            case btMultibodyLink::eRevolute:
+            {
+                //reset to current pos
+                pJointPos[0] = m_links[i].m_jointPos[0];
+                btScalar jointVel = pJointVel[0];
+                pJointPos[0] += dt * jointVel;
+                break;
             }
+            case btMultibodyLink::eSpherical:
+            {
+                //reset to current pos
+
+                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;
+                jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+                pQuatUpdateFun(jointVel, jointOri, false, dt);
+                pJointPos[0] = jointOri.x();
+                pJointPos[1] = jointOri.y();
+                pJointPos[2] = jointOri.z();
+                pJointPos[3] = jointOri.w();
+                break;
+            }
+            case btMultibodyLink::ePlanar:
+            {
+                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);
+                btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
+                pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
+                pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
+                break;
+            }
+            default:
+            {
+            }
+        }
+        
+        m_links[i].updateInterpolationCacheMultiDof();
+    }
+}
+
+void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
+{
+	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)
+    
+	pBasePos[0] += dt * pBaseVel[0];
+	pBasePos[1] += dt * pBaseVel[1];
+	pBasePos[2] += dt * pBaseVel[2];
+
+	///////////////////////////////
+	//local functor for quaternion integration (to avoid error prone redundancy)
+	struct
+	{
+		//"exponential map" based on btTransformUtil::integrateTransform(..)
+		void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
+		{
+			//baseBody	=>	quat is alias and omega is global coor
+			//!baseBody	=>	quat is alibi and omega is local coor
+
+			btVector3 axis;
+			btVector3 angvel;
+
+			if (!baseBody)
+				angvel = quatRotate(quat, omega);  //if quat is not m_baseQuat, it is alibi => ok
+			else
+				angvel = omega;
+
+			btScalar fAngle = angvel.length();
+			//limit the angular motion
+			if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
+			{
+				fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;
+			}
+
+			if (fAngle < btScalar(0.001))
+			{
+				// use Taylor's expansions of sync function
+				axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle);
+			}
+			else
+			{
+				// sync(fAngle) = sin(c*fAngle)/t
+				axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle);
+			}
+
+			if (!baseBody)
+				quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat;
+			else
+				quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5)));
+			//equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
+
+			quat.normalize();
+		}
+	} pQuatUpdateFun;
+	///////////////////////////////
+
+	//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
+	//
+    btScalar *pBaseQuat = pq ? pq : m_baseQuat;
 	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;
@@ -1714,10 +1835,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 	for (int i = 0; i < num_links; ++i)
 	{
         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));
 
@@ -1727,10 +1845,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 			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;
@@ -1738,11 +1852,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 			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;
@@ -1756,11 +1865,6 @@ 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);
@@ -1775,7 +1879,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 			}
 		}
 
-		m_links[i].updateCacheMultiDof(pq, predict);
+		m_links[i].updateCacheMultiDof(pq);
 
 		if (pq)
 			pq += m_links[i].m_posVarCount;

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -435,7 +435,7 @@ public:
 	}
 
 	// timestep the positions (given current velocities).
-	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0, bool predict = false);
+	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
     
     // predict the positions
     void predictPositionsMultiDof(btScalar dt);

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -36,7 +36,7 @@ void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
 void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
     btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
-    integrateMultiBodyTransforms(timeStep, /*predict = */ true);
+    predictMultiBodyTransforms(timeStep);
     
 }
 void btMultiBodyDynamicsWorld::calculateSimulationIslands()
@@ -791,7 +791,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
     integrateMultiBodyTransforms(timeStep);
 }
 
-void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, bool predict)
+void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
 {
 		BT_PROFILE("btMultiBody stepPositions");
 		//integrate and update the Featherstone hierarchies
@@ -815,8 +815,6 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, b
 				int nLinks = bod->getNumLinks();
 
 				///base + num m_links
-                if (!predict)
-				{
                 if (!bod->isPosUpdated())
                     bod->stepPositionsMultiDof(timeStep);
                 else
@@ -827,15 +825,10 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, b
                     bod->stepPositionsMultiDof(1, 0, pRealBuf);
                     bod->setPosUpdated(false);
                 }
-				}
+
-                else
-                    bod->predictPositionsMultiDof(timeStep);
 
 				m_scratch_world_to_local.resize(nLinks + 1);
 				m_scratch_local_origin.resize(nLinks + 1);
-                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
@@ -845,6 +838,40 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, b
 		}
 }
 
+void btMultiBodyDynamicsWorld::predictMultiBodyTransforms(btScalar timeStep)
+{
+    BT_PROFILE("btMultiBody stepPositions");
+    //integrate and update the Featherstone hierarchies
+    
+    for (int b = 0; b < m_multiBodies.size(); b++)
+    {
+        btMultiBody* bod = m_multiBodies[b];
+        bool isSleeping = false;
+        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+        {
+            isSleeping = true;
+        }
+        for (int b = 0; b < bod->getNumLinks(); b++)
+        {
+            if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                isSleeping = true;
+        }
+        
+        if (!isSleeping)
+        {
+            int nLinks = bod->getNumLinks();
+            bod->predictPositionsMultiDof(timeStep);
+            m_scratch_world_to_local.resize(nLinks + 1);
+            m_scratch_local_origin.resize(nLinks + 1);
+            bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+        }
+        else
+        {
+            bod->clearVelocities();
+        }
+    }
+}
+
 void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint)
 {
 	m_multiBodyConstraints.push_back(constraint);

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -96,7 +96,8 @@ public:
 	virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint);
 
 	virtual void integrateTransforms(btScalar timeStep);
-    void integrateMultiBodyTransforms(btScalar timeStep,bool predict = false);
+    void integrateMultiBodyTransforms(btScalar timeStep);
+    void predictMultiBodyTransforms(btScalar timeStep);
     
     virtual void predictUnconstraintMotion(btScalar timeStep);
 	virtual void debugDrawWorld();

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -191,16 +191,62 @@ struct btMultibodyLink
 	}
 
 	// routine to update m_cachedRotParentToThis and m_cachedRVector
-	void updateCacheMultiDof(btScalar *pq = 0, bool predict = false)
+	void updateCacheMultiDof(btScalar *pq = 0)
 	{
-        btScalar *pJointPos;
+        btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
+        btQuaternion& cachedRot = m_cachedRotParentToThis;
+        btVector3& cachedVector =m_cachedRVector;
+		switch (m_jointType)
+		{
+			case eRevolute:
+			{
+				cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
 
-        if (!predict)
+				break;
-            pJointPos = (pq ? pq : &m_jointPos[0]);
+			}
-        else
+			case ePrismatic:
-            pJointPos = &m_jointPos_interpolate[0];
+			{
-        btQuaternion& cachedRot = predict ? m_cachedRotParentToThis_interpolate : m_cachedRotParentToThis;
+				// m_cachedRotParentToThis never changes, so no need to update
-        btVector3& cachedVector = predict ? m_cachedRVector_interpolate : m_cachedRVector;
+				cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
+
+				break;
+			}
+			case eSpherical:
+			{
+				cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
+
+				break;
+			}
+			case ePlanar:
+			{
+				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:
+			{
+				cachedRot = m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
+
+				break;
+			}
+			default:
+			{
+				//invalid type
+				btAssert(0);
+			}
+		}
+	}
+    
+    void updateInterpolationCacheMultiDof()
+    {
+        btScalar *pJointPos = &m_jointPos_interpolate[0];
+        
+        btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate;
+        btVector3& cachedVector = m_cachedRVector_interpolate;
         switch (m_jointType)
         {
             case eRevolute: