made the multiDof-singleDof disctinction a bit cleaner

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -85,7 +85,8 @@ btMultiBody::btMultiBody(int n_links,
                      btScalar mass,
                      const btVector3 &inertia,
                      bool fixedBase,
-                     bool canSleep)
+                     bool canSleep,
+					 bool multiDof)
     : m_baseQuat(0, 0, 0, 1),
       m_baseMass(mass),
       m_baseInertia(inertia),
@@ -101,54 +102,24 @@ btMultiBody::btMultiBody(int n_links,
 		m_maxAppliedImpulse(1000.f),
 		m_hasSelfCollision(true),		
 		m_dofCount(n_links),
-		__posUpdated(false),
+		__posUpdated(false),		
-		m_posVarCnt(-1)				//-1 => not calculated yet/invalid
+		m_isMultiDof(multiDof),
+		m_posVarCnt(0)
 {
+	
+	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_vectorBuf.resize(2*n_links);
-    m_matrixBuf.resize(n_links + 1);
-	m_realBuf.resize(6 + 2*n_links);
-    m_basePos.setValue(0, 0, 0);
-    m_baseForce.setValue(0, 0, 0);
-    m_baseTorque.setValue(0, 0, 0);
-
-	m_isMultiDof = false;
-}
-
-btMultiBody::btMultiBody(int n_links, int n_dofs, btScalar mass,
-                     const btVector3 &inertia,
-                     bool fixedBase,
-                     bool canSleep)
-    : m_baseQuat(0, 0, 0, 1),
-      m_baseMass(mass),
-      m_baseInertia(inertia),
-    
-		m_fixedBase(fixedBase),
-		m_awake(true),
-		m_canSleep(canSleep),
-		m_sleepTimer(0),
-		m_baseCollider(0),
-		m_linearDamping(0.04f),
-		m_angularDamping(0.04f),
-		m_useGyroTerm(true),
-		m_maxAppliedImpulse(1000.f),
-		m_hasSelfCollision(true),
-		m_dofCount(n_dofs),
-		__posUpdated(false),
-		m_posVarCnt(-1)				//-1 => not calculated yet/invalid
-{
-	m_links.resize(n_links);
-
-	m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount);			//m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices
-	m_vectorBuf.resize(2 * m_dofCount);								//two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
 	m_matrixBuf.resize(n_links + 1);
 
-    m_basePos.setValue(0, 0, 0);
+	m_basePos.setValue(0, 0, 0);
     m_baseForce.setValue(0, 0, 0);
     m_baseTorque.setValue(0, 0, 0);
-
-	m_isMultiDof = n_links != n_dofs;
 }
 
 btMultiBody::~btMultiBody()
@@ -164,6 +135,12 @@ void btMultiBody::setupPrismatic(int i,
                                const btVector3 &parentComToThisComOffset,							   
 							   bool disableParentCollision)
 {
+	if(m_isMultiDof)
+	{
+		m_dofCount += 1;
+		m_posVarCnt += 1;
+	}
+
     m_links[i].m_mass = mass;
     m_links[i].m_inertia = inertia;
     m_links[i].m_parent = parent;
@@ -201,6 +178,12 @@ void btMultiBody::setupRevolute(int i,
                               const btVector3 &thisPivotToThisComOffset,
 							  bool disableParentCollision)
 {
+	if(m_isMultiDof)
+	{
+		m_dofCount += 1;
+		m_posVarCnt += 1;
+	}
+
     m_links[i].m_mass = mass;
     m_links[i].m_inertia = inertia;
     m_links[i].m_parent = parent;
@@ -240,6 +223,8 @@ void btMultiBody::setupSpherical(int i,
 						   bool disableParentCollision)
 {
 	btAssert(m_isMultiDof);
+	m_dofCount += 3;
+	m_posVarCnt += 4;
 
 	m_links[i].m_mass = mass;
     m_links[i].m_inertia = inertia;
@@ -280,6 +265,8 @@ void btMultiBody::setupPlanar(int i,
 						   bool disableParentCollision)
 {
 	btAssert(m_isMultiDof);
+	m_dofCount += 3;
+	m_posVarCnt += 3;
 
 	m_links[i].m_mass = mass;
     m_links[i].m_inertia = inertia;
@@ -315,14 +302,12 @@ void btMultiBody::setupPlanar(int i,
 }
 #endif
 
-void btMultiBody::forceMultiDof()
+void btMultiBody::finalizeMultiDof()
 {
-	if(m_isMultiDof) return;
+	btAssert(m_isMultiDof);
 
-	m_isMultiDof = true;
+	m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount);				//m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices
-
+	m_vectorBuf.resize(2 * m_dofCount);										//two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
-	m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount);		//m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices
-	m_vectorBuf.resize(2 * m_dofCount);								//two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
 
 	updateLinksDofOffsets();
 }

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -49,14 +49,9 @@ 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
-	btMultiBody(int n_links,             // NOT including the base
+			  );
-				int n_dofs,				// NOT including 6 floating base dofs
-				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);
 
     ~btMultiBody();	
     
@@ -356,20 +351,20 @@ public:
 		//	printf("%.4f ", delta_vee[dof]*multiplier);
 		//printf("\n");
 
-		btScalar sum = 0;
+		//btScalar sum = 0;
-        for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+		//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
-		{
+		//{
-			sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
+		//	sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
-		}
+		//}
-		btScalar l = btSqrt(sum);
+		//btScalar l = btSqrt(sum);
 
-		if (l>m_maxAppliedImpulse)
+		//if (l>m_maxAppliedImpulse)
-		{
+		//{
-			multiplier *= m_maxAppliedImpulse/l;
+		//	multiplier *= m_maxAppliedImpulse/l;
-		}
+		//}
 
-        for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
-		{			
+		{
 			m_realBuf[dof] += delta_vee[dof] * multiplier;
 		}
     }
@@ -480,7 +475,7 @@ public:
 	}
 
 	bool isMultiDof() { return m_isMultiDof; }
-	void forceMultiDof();
+	void finalizeMultiDof();
 
 	bool __posUpdated;
 
@@ -503,8 +498,6 @@ private:
 			m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset;
 			dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount;
 		}
-
-		m_posVarCnt = cfgOffset;
 	}
 
 	void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
@@ -529,7 +522,7 @@ private:
     //
     // realBuf:
     //  offset         size            array
-    //   0              6 + num_links   v (base_omega; base_vel; joint_vels)
+    //   0              6 + num_links   v (base_omega; base_vel; joint_vels)					MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized]
     //   6+num_links    num_links       D
     //
     // vectorBuf: