Only support btMultiBody multi-dof version (remove non-multi-dof path)

Use ATTRIBUTE_ALIGNED16 for btMultiBody
Always disable parentCollision for btMultiBody::setupFixed

examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h

@@ -15,7 +15,7 @@ public:
     ///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
     virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) = 0;
     
-    virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof)  =0;
+    virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep)  =0;
     
     virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape)  = 0;
     

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp

@@ -18,12 +18,12 @@ m_guiHelper(guiHelper)
 }
 
     
- class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof)
+ class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep)
 {
 //	m_urdf2mbLink.resize(totalNumJoints+1,-2);
     m_mb2urdfLink.resize(totalNumJoints+1,-2);
 
-    m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep,multiDof);
+    m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep);
     return m_bulletMultiBody;
 }
 

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h

@@ -43,7 +43,7 @@ public:
     ///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
     virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba);
     
-    virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof);
+    virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep);
     
     virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape);
     

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -241,11 +241,11 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
         {
             if (cache.m_bulletMultiBody==0)
             {
-                bool multiDof = true;
+                
                 bool canSleep = false;
                 bool isFixedBase = (mass==0);//todo: figure out when base is fixed
                 int totalNumJoints = cache.m_totalNumJoints1;
-                cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
+                cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep);
 
                 cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
             }
@@ -271,7 +271,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                         //b3Printf("Fixed joint (btMultiBody)\n");
                         btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
                         cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
-                                                               rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);
+                                                               rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin());
                         creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
 
 

examples/MultiBody/InvertedPendulumPDControl.cpp

@@ -136,8 +136,8 @@ void InvertedPendulumPDControl::initPhysics()
             delete shape;
         }
 
-        bool isMultiDof = true;
+        
-        btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep, isMultiDof);
+        btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep);
 		
         m_multiBody = pMultiBody;
         btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
@@ -201,7 +201,7 @@ void InvertedPendulumPDControl::initPhysics()
 					pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, 
 					btQuaternion(0.f, 0.f, 0.f, 1.f), 
 					parentComToCurrentPivot, 
-					currentPivotToCurrentCom, false);
+					currentPivotToCurrentCom);
 				}
 					
 				//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
@@ -249,10 +249,9 @@ void InvertedPendulumPDControl::initPhysics()
         {
             btScalar q0 = 180.f * SIMD_PI/ 180.f;
             if(!spherical)
-                if(mbC->isMultiDof())
+			{
-                    mbC->setJointPosMultiDof(0, &q0);
+				mbC->setJointPosMultiDof(0, &q0);
-                else
+			}
-                    mbC->setJointPos(0, q0);
             else
             {
                 btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);

examples/MultiBody/MultiBodyConstraintFeedback.cpp

@@ -131,8 +131,8 @@ void MultiBodyConstraintFeedbackSetup::initPhysics()
             delete shape;
         }
 
-        bool isMultiDof = true;
+        
-        btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
+        btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
 		
         m_multiBody = pMultiBody;
         btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
@@ -196,7 +196,7 @@ void MultiBodyConstraintFeedbackSetup::initPhysics()
 					pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, 
 					btQuaternion(0.f, 0.f, 0.f, 1.f), 
 					parentComToCurrentPivot, 
-					currentPivotToCurrentCom, false);
+					currentPivotToCurrentCom);
 				}
 					
 				//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
@@ -244,10 +244,9 @@ void MultiBodyConstraintFeedbackSetup::initPhysics()
         {
             btScalar q0 = 45.f * SIMD_PI/ 180.f;
             if(!spherical)
-                if(mbC->isMultiDof())
+			{
-                    mbC->setJointPosMultiDof(0, &q0);
+                   mbC->setJointPosMultiDof(0, &q0);
-                else
+			}
-                    mbC->setJointPos(0, q0);
             else
             {
                 btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);

examples/MultiBody/MultiDofDemo.cpp

@@ -161,10 +161,9 @@ void	MultiDofDemo::initPhysics()
 	{			
 		btScalar q0 = 45.f * SIMD_PI/ 180.f;
 		if(!spherical)
-			if(mbC->isMultiDof())
+		{
-				mbC->setJointPosMultiDof(0, &q0);
+			mbC->setJointPosMultiDof(0, &q0);
-			else
+		}
-				mbC->setJointPos(0, q0);
 		else
 		{
 			btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
@@ -260,8 +259,8 @@ btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyD
 	}
 
 	bool canSleep = false;
-	bool isMultiDof = true;
+	
-	btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
+	btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
 
 	btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
 	pMultiBody->setBasePos(basePosition);

examples/MultiBody/TestJointTorqueSetup.cpp

@@ -135,8 +135,8 @@ void TestJointTorqueSetup::initPhysics()
             delete shape;
         }
 
-        bool isMultiDof = true;
+
-        btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
+        btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
 		
         m_multiBody = pMultiBody;
         btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
@@ -200,7 +200,7 @@ void TestJointTorqueSetup::initPhysics()
 					pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, 
 					btQuaternion(0.f, 0.f, 0.f, 1.f), 
 					parentComToCurrentPivot, 
-					currentPivotToCurrentCom, false);
+					currentPivotToCurrentCom);
 				}
 					
 				//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
@@ -248,10 +248,9 @@ void TestJointTorqueSetup::initPhysics()
         {
             btScalar q0 = 45.f * SIMD_PI/ 180.f;
             if(!spherical)
-                if(mbC->isMultiDof())
+			{
-                    mbC->setJointPosMultiDof(0, &q0);
+				mbC->setJointPosMultiDof(0, &q0);
-                else
+			}
-                    mbC->setJointPos(0, q0);
             else
             {
                 btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -91,8 +91,7 @@ btMultiBody::btMultiBody(int n_links,
                      btScalar mass,
                      const btVector3 &inertia,
                      bool fixedBase,
-                     bool canSleep,
+                     bool canSleep)
-					 bool multiDof)
     : 
     	m_baseCollider(0),
 		m_baseName(0),
@@ -112,7 +111,6 @@ btMultiBody::btMultiBody(int n_links,
 			m_maxAppliedImpulse(1000.f),
 		m_maxCoordinateVelocity(100.f),
 			m_hasSelfCollision(true),		
-		m_isMultiDof(multiDof),
 		__posUpdated(false),
 			m_dofCount(0),
 		m_posVarCnt(0),
@@ -120,14 +118,6 @@ btMultiBody::btMultiBody(int n_links,
 		m_useGlobalVelocities(false),
 		m_internalNeedsJointFeedback(false)
 {
-	
-	if(!m_isMultiDof)
-	{
-		m_vectorBuf.resize(2*n_links);	
-		m_realBuf.resize(6 + 2*n_links);
-		m_posVarCnt = n_links;
-	}
-   
 	m_links.resize(n_links);
 	m_matrixBuf.resize(n_links + 1);
 
@@ -146,11 +136,9 @@ void btMultiBody::setupFixed(int i,
 						   int parent,
 						   const btQuaternion &rotParentToThis,
 						   const btVector3 &parentComToThisPivotOffset,
-                           const btVector3 &thisPivotToThisComOffset,
+                           const btVector3 &thisPivotToThisComOffset)
-						   bool disableParentCollision)
 {
-	btAssert(m_isMultiDof);
+	
-
 	m_links[i].m_mass = mass;
     m_links[i].m_inertiaLocal = inertia;
     m_links[i].m_parent = parent;
@@ -162,15 +150,10 @@ void btMultiBody::setupFixed(int i,
 	m_links[i].m_dofCount = 0;
 	m_links[i].m_posVarCount = 0;
 
-	if (disableParentCollision)
+	m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;    
-		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;    
+	
-	//
 	m_links[i].updateCacheMultiDof();
 
-	//
-	//if(m_isMultiDof)
-	//	resizeInternalMultiDofBuffers();
-	//
 	updateLinksDofOffsets();
 
 }
@@ -186,12 +169,9 @@ void btMultiBody::setupPrismatic(int i,
 							   const btVector3 &thisPivotToThisComOffset,
 							   bool disableParentCollision)
 {
-	if(m_isMultiDof)
+	m_dofCount += 1;
-	{
+	m_posVarCnt += 1;
-		m_dofCount += 1;
+	
-		m_posVarCnt += 1;
-	}
-
     m_links[i].m_mass = mass;
     m_links[i].m_inertiaLocal = inertia;
     m_links[i].m_parent = parent;
@@ -211,13 +191,10 @@ void btMultiBody::setupPrismatic(int i,
 	if (disableParentCollision)
 		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
 	//
-	if(m_isMultiDof)
+	
-		m_links[i].updateCacheMultiDof();
+	m_links[i].updateCacheMultiDof();
-	else
+	
-		m_links[i].updateCache();	
+	updateLinksDofOffsets();
-	//
-	if(m_isMultiDof)
-		updateLinksDofOffsets();
 }
 
 void btMultiBody::setupRevolute(int i,
@@ -230,12 +207,9 @@ void btMultiBody::setupRevolute(int i,
                               const btVector3 &thisPivotToThisComOffset,
 							  bool disableParentCollision)
 {
-	if(m_isMultiDof)
+	m_dofCount += 1;
-	{
+	m_posVarCnt += 1;
-		m_dofCount += 1;
+	
-		m_posVarCnt += 1;
-	}
-
     m_links[i].m_mass = mass;
     m_links[i].m_inertiaLocal = inertia;
     m_links[i].m_parent = parent;
@@ -254,13 +228,9 @@ void btMultiBody::setupRevolute(int i,
 	if (disableParentCollision)
 		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
     //
-	if(m_isMultiDof)
+	m_links[i].updateCacheMultiDof();
-		m_links[i].updateCacheMultiDof();
-	else
-		m_links[i].updateCache();	
 	//
-	if(m_isMultiDof)
+	updateLinksDofOffsets();
-		updateLinksDofOffsets();
 }
 
 
@@ -274,7 +244,7 @@ void btMultiBody::setupSpherical(int i,
 						   const btVector3 &thisPivotToThisComOffset,
 						   bool disableParentCollision)
 {
-	btAssert(m_isMultiDof);
+	
 	m_dofCount += 3;
 	m_posVarCnt += 4;
 
@@ -315,7 +285,7 @@ void btMultiBody::setupPlanar(int i,
 						   const btVector3 &parentComToThisComOffset,						   
 						   bool disableParentCollision)
 {
-	btAssert(m_isMultiDof);
+	
 	m_dofCount += 3;
 	m_posVarCnt += 3;
 
@@ -357,7 +327,6 @@ void btMultiBody::setupPlanar(int i,
 
 void btMultiBody::finalizeMultiDof()
 {
-	btAssert(m_isMultiDof);
 	m_deltaV.resize(0);
 	m_deltaV.resize(6 + m_dofCount);
 	m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount);			//m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
@@ -388,7 +357,7 @@ btScalar btMultiBody::getJointPos(int i) const
 
 btScalar btMultiBody::getJointVel(int i) const
 {
-    return m_realBuf[6 + i];
+    return m_realBuf[6 + m_links[i].m_dofOffset];
 }
 
 btScalar * btMultiBody::getJointPosMultiDof(int i)
@@ -415,7 +384,7 @@ const btScalar * btMultiBody::getJointVelMultiDof(int i) const
 void btMultiBody::setJointPos(int i, btScalar q)
 {
     m_links[i].m_jointPos[0] = q;
-    m_links[i].updateCache();
+    m_links[i].updateCacheMultiDof();
 }
 
 void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
@@ -428,7 +397,7 @@ void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
 
 void btMultiBody::setJointVel(int i, btScalar qdot)
 {
-    m_realBuf[6 + i] = qdot;
+    m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
 }
 
 void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot)
@@ -1939,41 +1908,6 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output
 	//int dummy = 0;
 }
 
-void btMultiBody::stepPositions(btScalar dt)
-{
-	int num_links = getNumLinks();
-    // step position by adding dt * velocity
-	btVector3 v = getBaseVel();
-    m_basePos += dt * v;
-
-    // "exponential map" method for the rotation
-    btVector3 base_omega = getBaseOmega();
-    const btScalar omega_norm = base_omega.norm();
-    const btScalar omega_times_dt = omega_norm * dt;
-    const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156
-    if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE) 
-	{
-        const btScalar xsq = omega_times_dt * omega_times_dt;     // |omega|^2 * dt^2
-        const btScalar sin_term = dt * (xsq / 48.0f - 0.5f);      // -sin(0.5*dt*|omega|) / |omega|
-        const btScalar cos_term = 1.0f - xsq / 8.0f;              // cos(0.5*dt*|omega|)
-        m_baseQuat = m_baseQuat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term);
-    } else 
-	{
-        m_baseQuat = m_baseQuat * btQuaternion(base_omega / omega_norm,-omega_times_dt);
-    }
-
-    // Make sure the quaternion represents a valid rotation.
-    // (Not strictly necessary, but helps prevent any round-off errors from building up.)
-    m_baseQuat.normalize();
-
-    // Finally we can update m_jointPos for each of the m_links
-    for (int i = 0; i < num_links; ++i) 
-	{
-		float jointVel = getJointVel(i);
-        m_links[i].m_jointPos[0] += dt * jointVel;
-        m_links[i].updateCache();
-    }
-}
 
 void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
 {	
@@ -2238,98 +2172,6 @@ void btMultiBody::filConstraintJacobianMultiDof(int link,
     }
 }
 
-void btMultiBody::fillContactJacobian(int link,
-                                    const btVector3 &contact_point,
-                                    const btVector3 &normal,
-                                    btScalar *jac,
-                                    btAlignedObjectArray<btScalar> &scratch_r,
-                                    btAlignedObjectArray<btVector3> &scratch_v,
-                                    btAlignedObjectArray<btMatrix3x3> &scratch_m) const
-{
-    // temporary space
-	int num_links = getNumLinks();
-    scratch_v.resize(2*num_links + 2);
-    scratch_m.resize(num_links + 1);
-
-    btVector3 * v_ptr = &scratch_v[0];
-    btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
-    btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
-    btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
-
-    scratch_r.resize(num_links);
-    btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
-
-    btMatrix3x3 * rot_from_world = &scratch_m[0];
-
-    const btVector3 p_minus_com_world = contact_point - m_basePos;
-
-    rot_from_world[0] = btMatrix3x3(m_baseQuat);
-
-    p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
-    n_local[0] = rot_from_world[0] * normal;
-    
-    // omega coeffients first.
-	if (this->m_fixedBase)
-	{
-		for (int i=0;i<6;i++)
-		{
-			jac[i]=0;
-		}
-	} else
-	{
-		    btVector3 omega_coeffs;
-
-		omega_coeffs = p_minus_com_world.cross(normal);
-		jac[0] = omega_coeffs[0];
-		jac[1] = omega_coeffs[1];
-		jac[2] = omega_coeffs[2];
-		// then v coefficients
-		jac[3] = normal[0];
-		jac[4] = normal[1];
-		jac[5] = normal[2];
-	}
-    // Set remaining jac values to zero for now.
-    for (int i = 6; i < 6 + num_links; ++i) {
-        jac[i] = 0;
-    }
-
-    // Qdot coefficients, if necessary.
-    if (num_links > 0 && link > -1) {
-
-        // TODO: speed this up -- don't calculate for m_links we don't need.
-        // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
-        // which is resulting in repeated work being done...)
-
-        // calculate required normals & positions in the local frames.
-        for (int i = 0; i < num_links; ++i) {
-
-            // transform to local frame
-            const int parent = m_links[i].m_parent;
-            const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
-            rot_from_world[i+1] = mtx * rot_from_world[parent+1];
-
-            n_local[i+1] = mtx * n_local[parent+1];
-            p_minus_com[i+1] = mtx * p_minus_com[parent+1] - m_links[i].m_cachedRVector;
-
-            // calculate the jacobian entry
-			if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
-                results[i] = n_local[i+1].dot( m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0) );
-            } else {
-                results[i] = n_local[i+1].dot( m_links[i].getAxisBottom(0) );
-            }
-        }
-
-        // Now copy through to output.
-		//printf("jac[%d] = ", link);
-        while (link != -1) 
-		{
-            jac[6 + link] = results[link];
-			//printf("%.2f\t", jac[6 + link]);
-            link = m_links[link].m_parent;
-        }
-		//printf("]\n");
-    }
-}
 
 void btMultiBody::wakeUp()
 {
@@ -2354,17 +2196,11 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
 
     // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
     btScalar motion = 0;
-	if(m_isMultiDof)
 	{
 		for (int i = 0; i < 6 + m_dofCount; ++i) 		
 			motion += m_realBuf[i] * m_realBuf[i];
 	}
-	else
+	
-	{
-		for (int i = 0; i < 6 + num_links; ++i) 		
-			motion += m_realBuf[i] * m_realBuf[i];		
-	}  
-    
 
     if (motion < SLEEP_EPSILON) {
         m_sleepTimer += timestep;

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -45,7 +45,7 @@
 #include "btMultiBodyLink.h"
 class btMultiBodyLinkCollider;
 
-class btMultiBody 
+ATTRIBUTE_ALIGNED16(class) btMultiBody 
 {
 public:
 
@@ -60,21 +60,19 @@ public:
 		btScalar mass,                // mass of base
 		const btVector3 &inertia,    // inertia of base, in base frame; assumed diagonal
 		bool fixedBase,           // whether the base is fixed (true) or can move (false)
-		bool canSleep,
+		bool canSleep);
-		bool multiDof = false
-			  );
 
 
 	virtual ~btMultiBody();
     
+	//note: fixed link collision with parent is always disabled
 	void setupFixed(int linkIndex,
 						   btScalar mass,
 						   const btVector3 &inertia,
 						   int parent,
 						   const btQuaternion &rotParentToThis,
 						   const btVector3 &parentComToThisPivotOffset,
-                           const btVector3 &thisPivotToThisComOffset,
+                           const btVector3 &thisPivotToThisComOffset);
-						   bool disableParentCollision);
 
 						
 	void setupPrismatic(int i,
@@ -447,7 +445,6 @@ void addJointTorque(int i, btScalar Q);
 	
 	
     // timestep the positions (given current velocities).
-    void stepPositions(btScalar dt);
 	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
 
 
@@ -458,15 +455,7 @@ void addJointTorque(int i, btScalar Q);
     // This routine fills out a contact constraint jacobian for this body.
     // the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
     // 'normal' & 'contact_point' are both given in world coordinates.
-    void fillContactJacobian(int link,
+	
-                             const btVector3 &contact_point,
-                             const btVector3 &normal,
-                             btScalar *jac,
-                             btAlignedObjectArray<btScalar> &scratch_r,
-                             btAlignedObjectArray<btVector3> &scratch_v,
-                             btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
-
-	//multidof version of fillContactJacobian
 	void fillContactJacobianMultiDof(int link,
                              const btVector3 &contact_point,
                              const btVector3 &normal,
@@ -576,7 +565,7 @@ void addJointTorque(int i, btScalar Q);
 		return m_hasSelfCollision;
 	}
 
-	bool isMultiDof() { return m_isMultiDof; }
+	
 	void finalizeMultiDof();
 
 	void useRK4Integration(bool use) { m_useRK4 = use; }
@@ -700,7 +689,7 @@ private:
 	btScalar	m_maxAppliedImpulse;
 	btScalar	m_maxCoordinateVelocity;
 	bool		m_hasSelfCollision;
-	bool		m_isMultiDof;
+	
 		bool __posUpdated;
 		int m_dofCount, m_posVarCnt;
 	bool m_useRK4, m_useGlobalVelocities;

src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -23,18 +23,12 @@ void btMultiBodyConstraint::updateJacobianSizes()
 {
     if(m_bodyA)
 	{
-		if(m_bodyA->isMultiDof())
+		m_jacSizeA = (6 + m_bodyA->getNumDofs());
-			m_jacSizeA = (6 + m_bodyA->getNumDofs());
-		else
-			m_jacSizeA = (6 + m_bodyA->getNumLinks());
 	}
 
 	if(m_bodyB)
 	{
-		if(m_bodyB->isMultiDof())
+		m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
-			m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
-		else
-			m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumLinks();
 	}
 	else
 		m_jacSizeBoth = m_jacSizeA;
@@ -101,7 +95,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 			rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
 		}
 
-		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+		const int ndofA  = multiBodyA->getNumDofs() + 6;
 
 		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 
@@ -128,10 +122,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 		else
 		{
 			btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
-			if(multiBodyA->isMultiDof())
+			multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
-				multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
-			else
-				multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
 		}
 
 		//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
@@ -140,10 +131,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
 		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 		//determine..
-		if(multiBodyA->isMultiDof())
+		multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
-			multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
-		else
-			multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
 
 		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
@@ -167,7 +155,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 			rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
 		}
 
-		const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
+		const int ndofB  = multiBodyB->getNumDofs() + 6;
 
 		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
 		if (solverConstraint.m_deltaVelBindex <0)
@@ -189,10 +177,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 		}
 		else
 		{
-			if(multiBodyB->isMultiDof())
+			multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
-				multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
-			else
-				multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
 		}
 
 		//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
@@ -201,10 +186,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
 		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 		//determine..
-		if(multiBodyB->isMultiDof())
+		multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
-			multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
-		else
-			multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
 
 		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
@@ -231,7 +213,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 		//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
 		if (multiBodyA)
 		{
-			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+			ndofA = multiBodyA->getNumDofs() + 6;
 			jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
 			deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
@@ -249,7 +231,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 		//
 		if (multiBodyB)
 		{
-			const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
+			const int ndofB = multiBodyB->getNumDofs() + 6;
 			jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
 			deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
@@ -290,7 +272,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 		btVector3 vel1,vel2;
 		if (multiBodyA)
 		{
-			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+			ndofA = multiBodyA->getNumDofs() + 6;
 			btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA ; ++i)
 				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
@@ -301,7 +283,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 		}
 		if (multiBodyB)
 		{
-			ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
+			ndofB = multiBodyB->getNumDofs() + 6;
 			btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB ; ++i)
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -123,7 +123,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 
 	if (c.m_multiBodyA)
 	{
-		ndofA  = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6;
+		ndofA  = c.m_multiBodyA->getNumDofs() + 6;
 		for (int i = 0; i < ndofA; ++i) 
 			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
 	} else if(c.m_solverBodyIdA >= 0)
@@ -134,7 +134,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 
 	if (c.m_multiBodyB)
 	{
-		ndofB  = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6;
+		ndofB  = c.m_multiBodyB->getNumDofs() + 6;
 		for (int i = 0; i < ndofB; ++i) 
 			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
 	} else if(c.m_solverBodyIdB >= 0)
@@ -169,10 +169,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
-		if(c.m_multiBodyA->isMultiDof())
+		c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
-			c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
-		else
-			c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 	} else if(c.m_solverBodyIdA >= 0)
 	{
@@ -185,10 +182,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
-		if(c.m_multiBodyB->isMultiDof())
+		c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
-			c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
-		else
-			c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 	} else if(c.m_solverBodyIdB >= 0)
 	{
@@ -209,14 +203,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(con
 
 	if (c.m_multiBodyA)
 	{
-		ndofA  = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6;
+		ndofA  = c.m_multiBodyA->getNumDofs() + 6;
 		for (int i = 0; i < ndofA; ++i) 
 			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
 	}
 
 	if (c.m_multiBodyB)
 	{
-		ndofB  = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6;
+		ndofB  = c.m_multiBodyB->getNumDofs() + 6;
 		for (int i = 0; i < ndofB; ++i) 
 			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
 	}
@@ -296,7 +290,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		{
 			rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
 		}
-		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+		const int ndofA  = multiBodyA->getNumDofs() + 6;
 
 		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 
@@ -316,15 +310,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 
 		btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
-		if(multiBodyA->isMultiDof())
+		multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
-			multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
-		else
-			multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
 		btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-		if(multiBodyA->isMultiDof())
+		multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
-			multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
-		else
-			multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
 
 		btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
@@ -349,7 +337,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
 		}
 
-		const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
+		const int ndofB  = multiBodyB->getNumDofs() + 6;
 
 		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
 		if (solverConstraint.m_deltaVelBindex <0)
@@ -365,14 +353,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
 		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 
-		if(multiBodyB->isMultiDof())
+		multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
-			multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+		multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
-		else
-			multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
-		if(multiBodyB->isMultiDof())
-			multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
-		else
-			multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
 		
 		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);		
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
@@ -399,7 +381,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		int ndofA  = 0;
 		if (multiBodyA)
 		{
-			ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+			ndofA  = multiBodyA->getNumDofs() + 6;
 			jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 			lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
@@ -418,7 +400,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 		if (multiBodyB)
 		{
-			const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
+			const int ndofB  = multiBodyB->getNumDofs() + 6;
 			jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
 			lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
@@ -467,7 +449,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		btVector3 vel1,vel2;
 		if (multiBodyA)
 		{
-			ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+			ndofA  = multiBodyA->getNumDofs() + 6;
 			btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA ; ++i) 
 				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
@@ -480,7 +462,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 		if (multiBodyB)
 		{
-			ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
+			ndofB  = multiBodyB->getNumDofs() + 6;
 			btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB ; ++i) 
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
@@ -518,10 +500,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			{
 				btScalar impulse = solverConstraint.m_appliedImpulse;
 				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-				if(multiBodyA->isMultiDof())
+				multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
-					multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
+				
-				else
-					multiBodyA->applyDeltaVee(deltaV,impulse);
 				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
 			} else
 			{
@@ -532,10 +512,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			{
 				btScalar impulse = solverConstraint.m_appliedImpulse;
 				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-				if(multiBodyB->isMultiDof())
+				multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
-					multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
-				else
-					multiBodyB->applyDeltaVee(deltaV,impulse);
 				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
 			} else
 			{
@@ -904,45 +881,39 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
 	if (c.m_multiBodyA)
 	{
 		
-		if(c.m_multiBodyA->isMultiDof())
+		c.m_multiBodyA->setCompanionId(-1);
+		btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
+		btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
+		if (c.m_linkA<0)
 		{
-			c.m_multiBodyA->setCompanionId(-1);
+			c.m_multiBodyA->addBaseConstraintForce(force);
-			btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
+			c.m_multiBodyA->addBaseConstraintTorque(torque);
-			btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
+		} else
-			if (c.m_linkA<0)
+		{
-			{
+			c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
-				c.m_multiBodyA->addBaseConstraintForce(force);
+				//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
-				c.m_multiBodyA->addBaseConstraintTorque(torque);
+			c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
-			} else
-			{
-				c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
-					//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
-				c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
-			}
 		}
 	}
 	
 	if (c.m_multiBodyB)
 	{
-		if(c.m_multiBodyB->isMultiDof())
 		{
+			c.m_multiBodyB->setCompanionId(-1);
+			btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
+			btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
+			if (c.m_linkB<0)
+			{
+				c.m_multiBodyB->addBaseConstraintForce(force);
+				c.m_multiBodyB->addBaseConstraintTorque(torque);
+			} else
 			{
-				c.m_multiBodyB->setCompanionId(-1);
-				btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
-				btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
-				if (c.m_linkB<0)
 				{
-					c.m_multiBodyB->addBaseConstraintForce(force);
+					c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force);
-					c.m_multiBodyB->addBaseConstraintTorque(torque);
+					//b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
-				} else
+					c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque);
-				{
-					{
-						c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force);
-						//b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
-						c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque);
-					}
-					
 				}
+					
 			}
 		}
 	}

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -505,7 +505,6 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 				scratch_m.resize(bod->getNumLinks()+1);
 				bool doNotUpdatePos = false;
 
-				if(bod->isMultiDof())
 				{
 					if(!bod->isUsingRK4Integration())
 					{
@@ -667,10 +666,7 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 						
 					}
 				}
-				else//if(bod->isMultiDof())
+				
-				{
-					bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
-				}
 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 				bod->clearForcesAndTorques();
 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
@@ -709,13 +705,13 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
                                 scratch_v.resize(bod->getNumLinks()+1);
                                 scratch_m.resize(bod->getNumLinks()+1);
 
-                                if(bod->isMultiDof())
+                                
+                            {
+                                if(!bod->isUsingRK4Integration())
                                 {
-                                        if(!bod->isUsingRK4Integration())
+									bool isConstraintPass = true;
-                                        {
+                                    bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass);
-						bool isConstraintPass = true;
+                                }
-                                                bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass);
-                                        }
 				}
 			}
 		}
@@ -760,7 +756,7 @@ void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 
 				///base + num m_links
 			
-				if(bod->isMultiDof())
+				
 				{
 					if(!bod->isPosUpdated())
 						bod->stepPositionsMultiDof(timeStep);
@@ -773,10 +769,7 @@ void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 						bod->setPosUpdated(false);
 					}
 				}
-				else
+				
-				{
-					bod->stepPositions(timeStep);			
-				}
 				world_to_local.resize(nLinks+1);
 				local_origin.resize(nLinks+1);
 

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -38,7 +38,7 @@ void btMultiBodyJointLimitConstraint::finalizeMultiDof()
 
 	allocateJacobiansMultiDof();
 
-	unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset : m_linkA);
+	unsigned int offset = 6 + m_bodyA->getLink(m_linkA).m_dofOffset;
 
 	// row 0: the lower bound
 	jacobianA(0)[offset] = 1;
@@ -124,7 +124,6 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 
 		btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
 
-		if (m_bodyA->isMultiDof())
 		{
 			//expect either prismatic or revolute joint type for now
 			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -39,7 +39,7 @@ void btMultiBodyJointMotor::finalizeMultiDof()
 	// note: we rely on the fact that data.m_jacobians are
 	// always initialized to zero by the Constraint ctor
 	int linkDoF = 0;
-	unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF : m_linkA);
+	unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
 
 	// row 0: the lower bound
 	// row 0: the lower bound
@@ -118,7 +118,6 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
 		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
 		constraintRow.m_orgConstraint = this;
 		constraintRow.m_orgDofIndex = row;
-		if (m_bodyA->isMultiDof())
 		{
 			//expect either prismatic or revolute joint type for now
 			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -165,20 +165,6 @@ btVector3 m_appliedConstraintForce;    // In WORLD frame
 	}
 
     // routine to update m_cachedRotParentToThis and m_cachedRVector
-    void updateCache()
-	{
-		//multidof
-		if (m_jointType == eRevolute) 
-		{
-			m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis;
-			m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
-		} else 
-		{
-			// m_cachedRotParentToThis never changes, so no need to update
-			m_cachedRVector = m_eVector + m_jointPos[0] * getAxisBottom(0);
-		}
-	}
-
 	void updateCacheMultiDof(btScalar *pq = 0)
 	{
 		btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);