Merge branch 'master' of http://github.com/kalesony/bullet3

Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp

@@ -408,7 +408,7 @@ btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDyn
 				{	
 					if (1)
 					{
-						btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,500000); 
+						btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,0,500000); 
 						world->addMultiBodyConstraint(con);
 					}
 

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -48,8 +48,10 @@ public:
     btMultiBody(int n_links,             // NOT including the base
               btScalar mass,                // mass of base
               const btVector3 &inertia,    // inertia of base, in base frame; assumed diagonal
-              bool fixed_base_,           // whether the base is fixed (true) or can move (false)
+              bool fixedBase,           // whether the base is fixed (true) or can move (false)
-              bool can_sleep_);
+              bool canSleep,
+			  bool multiDof = false
+			  );
 
     ~btMultiBody();	
     
@@ -57,9 +59,9 @@ public:
                         btScalar mass,
                         const btVector3 &inertia,       // in my frame; assumed diagonal
                         int parent,
-                        const btQuaternion &rot_parent_to_this,  // rotate points in parent frame to my frame.
+                        const btQuaternion &rotParentToThis,  // rotate points in parent frame to my frame.
-                        const btVector3 &joint_axis,             // in my frame
+                        const btVector3 &jointAxis,             // in my frame
-                        const btVector3 &r_vector_when_q_zero,  // vector from parent COM to my COM, in my frame, when q = 0.
+                        const btVector3 &parentComToThisComOffset,  // vector from parent COM to my COM, in my frame, when q = 0.
 						bool disableParentCollision=false
 						);
 
@@ -67,20 +69,40 @@ public:
                        btScalar mass,
                        const btVector3 &inertia,
                        int parent,
-                       const btQuaternion &zero_rot_parent_to_this,  // rotate points in parent frame to this frame, when q = 0
+                       const btQuaternion &rotParentToThis,  // rotate points in parent frame to this frame, when q = 0
-                       const btVector3 &joint_axis,    // in my frame
+                       const btVector3 &jointAxis,    // in my frame
-                       const btVector3 &parent_axis_position,    // vector from parent COM to joint axis, in PARENT frame
+                       const btVector3 &parentComToThisPivotOffset,    // vector from parent COM to joint axis, in PARENT frame
-                       const btVector3 &my_axis_position,       // vector from joint axis to my COM, in MY frame
+                       const btVector3 &thisPivotToThisComOffset,       // vector from joint axis to my COM, in MY frame
 					   bool disableParentCollision=false);
 
+	void setupSpherical(int i,											// 0 to num_links-1
+                       btScalar mass,
+                       const btVector3 &inertia,
+                       int parent,
+                       const btQuaternion &rotParentToThis,		// rotate points in parent frame to this frame, when q = 0                       
+                       const btVector3 &parentComToThisPivotOffset,			// vector from parent COM to joint axis, in PARENT frame
+                       const btVector3 &thisPivotToThisComOffset,				// vector from joint axis to my COM, in MY frame
+					   bool disableParentCollision=false);		
+
+#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+	void setupPlanar(int i,											// 0 to num_links-1
+                       btScalar mass,
+                       const btVector3 &inertia,
+                       int parent,
+                       const btQuaternion &rotParentToThis,		// rotate points in parent frame to this frame, when q = 0                       
+					   const btVector3 &rotationAxis,
+                       const btVector3 &parentComToThisComOffset,			// vector from parent COM to this COM, in PARENT frame                       
+					   bool disableParentCollision=false);		
+#endif
+	
 	const btMultibodyLink& getLink(int index) const
 	{
-		return links[index];
+		return m_links[index];
 	}
 
 	btMultibodyLink& getLink(int index)
 	{
-		return links[index];
+		return m_links[index];
 	}
 
 
@@ -106,12 +128,14 @@ public:
     
     
     //
-    // get number of links, masses, moments of inertia
+    // get number of m_links, masses, moments of inertia
     //
 
-    int getNumLinks() const { return links.size(); }
+    int getNumLinks() const { return m_links.size(); }
-    btScalar getBaseMass() const { return base_mass; }
+	int getNumDofs() const { return m_dofCount; }
-    const btVector3 & getBaseInertia() const { return base_inertia; }
+	int getNumPosVars() const { return m_posVarCnt; }
+    btScalar getBaseMass() const { return m_baseMass; }
+    const btVector3 & getBaseInertia() const { return m_baseInertia; }
     btScalar getLinkMass(int i) const;
     const btVector3 & getLinkInertia(int i) const;
     
@@ -120,55 +144,60 @@ public:
     // change mass (incomplete: can only change base mass and inertia at present)
     //
 
-    void setBaseMass(btScalar mass) { base_mass = mass; }
+    void setBaseMass(btScalar mass) { m_baseMass = mass; }
-    void setBaseInertia(const btVector3 &inertia) { base_inertia = inertia; }
+    void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; }
 
 
     //
     // get/set pos/vel/rot/omega for the base link
     //
 
-    const btVector3 & getBasePos() const { return base_pos; }    // in world frame
+    const btVector3 & getBasePos() const { return m_basePos; }    // in world frame
     const btVector3 getBaseVel() const 
 	{ 
-		return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]); 
+		return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]); 
 	}     // in world frame
     const btQuaternion & getWorldToBaseRot() const 
 	{ 
-		return base_quat; 
+		return m_baseQuat; 
 	}     // rotates world vectors into base frame
-    btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); }   // in world frame
+    btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); }   // in world frame
 
     void setBasePos(const btVector3 &pos) 
 	{ 
-		base_pos = pos; 
+		m_basePos = pos; 
 	}
     void setBaseVel(const btVector3 &vel) 
 	{ 
 
-		m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2]; 
+		m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2]; 
 	}
     void setWorldToBaseRot(const btQuaternion &rot) 
 	{ 
-		base_quat = rot; 
+		m_baseQuat = rot;					//m_baseQuat asumed to ba alias!?
 	}
     void setBaseOmega(const btVector3 &omega) 
 	{ 
-		m_real_buf[0]=omega[0]; 
+		m_realBuf[0]=omega[0]; 
-		m_real_buf[1]=omega[1]; 
+		m_realBuf[1]=omega[1]; 
-		m_real_buf[2]=omega[2]; 
+		m_realBuf[2]=omega[2]; 
 	}
 
 
     //
-    // get/set pos/vel for child links (i = 0 to num_links-1)
+    // get/set pos/vel for child m_links (i = 0 to num_links-1)
     //
 
     btScalar getJointPos(int i) const;
     btScalar getJointVel(int i) const;
 
+	btScalar * getJointVelMultiDof(int i);
+	btScalar * getJointPosMultiDof(int i);
+
     void setJointPos(int i, btScalar q);
     void setJointVel(int i, btScalar qdot);
+	void setJointPosMultiDof(int i, btScalar *q);
+    void setJointVelMultiDof(int i, btScalar *qdot);	
 
     //
     // direct access to velocities as a vector of 6 + num_links elements.
@@ -176,7 +205,7 @@ public:
     //
     const btScalar * getVelocityVector() const 
 	{ 
-		return &m_real_buf[0]; 
+		return &m_realBuf[0]; 
 	}
 /*    btScalar * getVelocityVector() 
 	{ 
@@ -185,7 +214,7 @@ public:
   */  
 
     //
-    // get the frames of reference (positions and orientations) of the child links
+    // get the frames of reference (positions and orientations) of the child m_links
     // (i = 0 to num_links-1)
     //
 
@@ -220,18 +249,21 @@ public:
 
     void addBaseForce(const btVector3 &f) 
 	{ 
-		base_force += f; 
+		m_baseForce += f; 
 	}
-    void addBaseTorque(const btVector3 &t) { base_torque += t; }
+    void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
     void addLinkForce(int i, const btVector3 &f);
     void addLinkTorque(int i, const btVector3 &t);
     void addJointTorque(int i, btScalar Q);
+	void addJointTorqueMultiDof(int i, int dof, btScalar Q);
+	void addJointTorqueMultiDof(int i, const btScalar *Q);
 
-    const btVector3 & getBaseForce() const { return base_force; }
+    const btVector3 & getBaseForce() const { return m_baseForce; }
-    const btVector3 & getBaseTorque() const { return base_torque; }
+    const btVector3 & getBaseTorque() const { return m_baseTorque; }
     const btVector3 & getLinkForce(int i) const;
     const btVector3 & getLinkTorque(int i) const;
     btScalar getJointTorque(int i) const;
+	btScalar * getJointTorqueMultiDof(int i);
 
 
     //
@@ -255,6 +287,11 @@ public:
                         btAlignedObjectArray<btVector3> &scratch_v,
                         btAlignedObjectArray<btMatrix3x3> &scratch_m);
 
+	void stepVelocitiesMultiDof(btScalar dt,
+                        btAlignedObjectArray<btScalar> &scratch_r,
+                        btAlignedObjectArray<btVector3> &scratch_v,
+                        btAlignedObjectArray<btMatrix3x3> &scratch_m);
+
     // calcAccelerationDeltas
     // input: force vector (in same format as jacobian, i.e.:
     //                      3 torque values, 3 force values, num_links joint torque values)
@@ -265,13 +302,17 @@ public:
                                 btAlignedObjectArray<btScalar> &scratch_r,
                                 btAlignedObjectArray<btVector3> &scratch_v) const;
 
+	void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
+                                btAlignedObjectArray<btScalar> &scratch_r,
+                                btAlignedObjectArray<btVector3> &scratch_v) const;
+
     // apply a delta-vee directly. used in sequential impulses code.
     void applyDeltaVee(const btScalar * delta_vee) 
 	{
 
         for (int i = 0; i < 6 + getNumLinks(); ++i) 
 		{
-			m_real_buf[i] += delta_vee[i];
+			m_realBuf[i] += delta_vee[i];
 		}
 		
     }
@@ -300,12 +341,37 @@ public:
         for (int i = 0; i < 6 + getNumLinks(); ++i)
 		{
 			sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
-			m_real_buf[i] += delta_vee[i] * multiplier;
+			m_realBuf[i] += delta_vee[i] * multiplier;
+		}
+    }
+
+	void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier) 
+	{
+		//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+		//	printf("%.4f ", delta_vee[dof]*multiplier);
+		//printf("\n");
+
+		//btScalar sum = 0;
+		//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+		//{
+		//	sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
+		//}
+		//btScalar l = btSqrt(sum);
+
+		//if (l>m_maxAppliedImpulse)
+		//{
+		//	multiplier *= m_maxAppliedImpulse/l;
+		//}
+
+		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+		{
+			m_realBuf[dof] += delta_vee[dof] * multiplier;
 		}
     }
 
     // timestep the positions (given current velocities).
     void stepPositions(btScalar dt);
+	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
 
 
     //
@@ -323,23 +389,43 @@ public:
                              btAlignedObjectArray<btVector3> &scratch_v,
                              btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
 
+	//multidof version of fillContactJacobian
+	void fillContactJacobianMultiDof(int link,
+                             const btVector3 &contact_point,
+                             const btVector3 &normal,
+                             btScalar *jac,
+                             btAlignedObjectArray<btScalar> &scratch_r,
+                             btAlignedObjectArray<btVector3> &scratch_v,
+							 btAlignedObjectArray<btMatrix3x3> &scratch_m) const { filConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
+
+	//a more general version of fillContactJacobianMultiDof which does not assume..
+	//.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
+	void filConstraintJacobianMultiDof(int link,
+                             const btVector3 &contact_point,
+							 const btVector3 &normal_ang,
+                             const btVector3 &normal_lin,
+                             btScalar *jac,
+                             btAlignedObjectArray<btScalar> &scratch_r,
+                             btAlignedObjectArray<btVector3> &scratch_v,
+                             btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
+
 
     //
     // sleeping
     //
 	void	setCanSleep(bool canSleep)
 	{
-		can_sleep = canSleep;
+		m_canSleep = canSleep;
 	}
 
-    bool isAwake() const { return awake; }
+    bool isAwake() const { return m_awake; }
     void wakeUp();
     void goToSleep();
     void checkMotionAndSleepIfRequired(btScalar timestep);
     
 	bool hasFixedBase() const
 	{
-		    return fixed_base;
+		    return m_fixedBase;
 	}
 
 	int getCompanionId() const
@@ -352,9 +438,9 @@ public:
 		m_companionId = id;
 	}
 
-	void setNumLinks(int numLinks)//careful: when changing the number of links, make sure to re-initialize or update existing links
+	void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
 	{
-		links.resize(numLinks);
+		m_links.resize(numLinks);
 	}
 
 	btScalar getLinearDamping() const
@@ -369,6 +455,10 @@ public:
 	{
 		return m_angularDamping;
 	}
+	void setAngularDamping( btScalar damp)
+	{
+		m_angularDamping = damp;
+	}
 		
 	bool getUseGyroTerm() const
 	{
@@ -396,6 +486,16 @@ public:
 		return m_hasSelfCollision;
 	}
 
+	bool isMultiDof() { return m_isMultiDof; }
+	void finalizeMultiDof();
+
+	void useRK4Integration(bool use) { m_useRK4 = use; }
+	bool isUsingRK4Integration() const { return m_useRK4; }
+	void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
+	bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; }
+
+	bool __posUpdated;
+
 private:
     btMultiBody(const btMultiBody &);  // not implemented
     void operator=(const btMultiBody &);  // not implemented
@@ -403,57 +503,74 @@ private:
     void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
 
 	void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
+#ifdef TEST_SPATIAL_ALGEBRA_LAYER
+	void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
+#endif
 	
+	void updateLinksDofOffsets()
+	{
+		int dofOffset = 0, cfgOffset = 0;
+		for(int bidx = 0; bidx < m_links.size(); ++bidx)
+		{
+			m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset;
+			dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount;
+		}
+	}
+
+	void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
 	
 private:
 
 	btMultiBodyLinkCollider* m_baseCollider;//can be NULL
 
-    btVector3 base_pos;       // position of COM of base (world frame)
+    btVector3 m_basePos;       // position of COM of base (world frame)
-    btQuaternion base_quat;   // rotates world points into base frame
+    btQuaternion m_baseQuat;   // rotates world points into base frame
 
-    btScalar base_mass;         // mass of the base
+    btScalar m_baseMass;         // mass of the base
-    btVector3 base_inertia;   // inertia of the base (in local frame; diagonal)
+    btVector3 m_baseInertia;   // inertia of the base (in local frame; diagonal)
 
-    btVector3 base_force;     // external force applied to base. World frame.
+    btVector3 m_baseForce;     // external force applied to base. World frame.
-    btVector3 base_torque;    // external torque applied to base. World frame.
+    btVector3 m_baseTorque;    // external torque applied to base. World frame.
     
-    btAlignedObjectArray<btMultibodyLink> links;    // array of links, excluding the base. index from 0 to num_links-1.
+    btAlignedObjectArray<btMultibodyLink> m_links;    // array of m_links, excluding the base. index from 0 to num_links-1.
 	btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
+	int m_dofCount, m_posVarCnt;
+
+	bool m_useRK4, m_useGlobalVelocities;
     
     //
-    // real_buf:
+    // 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
     //
-    // vector_buf:
+    // vectorBuf:
     //  offset         size         array
     //   0              num_links    h_top
     //   num_links      num_links    h_bottom
     //
-    // matrix_buf:
+    // matrixBuf:
     //  offset         size         array
     //   0              num_links+1  rot_from_parent
     //
     
-    btAlignedObjectArray<btScalar> m_real_buf;
+    btAlignedObjectArray<btScalar> m_realBuf;
-    btAlignedObjectArray<btVector3> vector_buf;
+    btAlignedObjectArray<btVector3> m_vectorBuf;
-    btAlignedObjectArray<btMatrix3x3> matrix_buf;
+    btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
 
     //std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
 
-	btMatrix3x3 cached_inertia_top_left;
+	btMatrix3x3 m_cachedInertiaTopLeft;
-	btMatrix3x3 cached_inertia_top_right;
+	btMatrix3x3 m_cachedInertiaTopRight;
-	btMatrix3x3 cached_inertia_lower_left;
+	btMatrix3x3 m_cachedInertiaLowerLeft;
-	btMatrix3x3 cached_inertia_lower_right;
+	btMatrix3x3 m_cachedInertiaLowerRight;
 
-    bool fixed_base;
+    bool m_fixedBase;
 
     // Sleep parameters.
-    bool awake;
+    bool m_awake;
-    bool can_sleep;
+    bool m_canSleep;
-    btScalar sleep_timer;
+    btScalar m_sleepTimer;
 
 	int	m_companionId;
 	btScalar	m_linearDamping;
@@ -461,6 +578,7 @@ private:
 	bool	m_useGyroTerm;
 	btScalar	m_maxAppliedImpulse;
 	bool		m_hasSelfCollision;
+	bool		m_isMultiDof;
 };
 
 #endif

src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -1,263 +1,85 @@
 #include "btMultiBodyConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "btMultiBodyPoint2Point.h"				//for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
 
 btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
 	:m_bodyA(bodyA),
 	m_bodyB(bodyB),
 	m_linkA(linkA),
 	m_linkB(linkB),
-	m_num_rows(numRows),
+	m_numRows(numRows),
 	m_isUnilateral(isUnilateral),
-	m_maxAppliedImpulse(100)
+	m_maxAppliedImpulse(100),
+	m_jacSizeA(0),
+	m_jacSizeBoth(0)
 {
-	m_jac_size_A = (6 + bodyA->getNumLinks());
+	
-	m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0));
+	if(bodyA)
-	m_pos_offset = ((1 + m_jac_size_both)*m_num_rows);
+	{
-	m_data.resize((2 + m_jac_size_both) * m_num_rows);
+		if(bodyA->isMultiDof())
+			m_jacSizeA = (6 + bodyA->getNumDofs());
+		else
+			m_jacSizeA = (6 + bodyA->getNumLinks());
+	}
+
+	if(bodyB)
+	{
+		if(bodyB->isMultiDof())
+			m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs();
+		else
+			m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks();
+	}
+	else
+		m_jacSizeBoth = m_jacSizeA;
+	
+	m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
+	m_data.resize((2 + m_jacSizeBoth) * m_numRows);
 }
 
 btMultiBodyConstraint::~btMultiBodyConstraint()
 {
 }
 
-
-
-btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
-														btMultiBodyJacobianData& data,
-														btScalar* jacOrgA,btScalar* jacOrgB,
-														const btContactSolverInfo& infoGlobal,
-														btScalar desiredVelocity,
-														btScalar lowerLimit,
-														btScalar upperLimit)
-{
-			
-	
-	
-	constraintRow.m_multiBodyA = m_bodyA;
-	constraintRow.m_multiBodyB = m_bodyB;
-
-	btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
-	btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
-
-	if (multiBodyA)
-	{
-		
-		const int ndofA  = multiBodyA->getNumLinks() + 6;
-
-		constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
-
-		if (constraintRow.m_deltaVelAindex <0)
-		{
-			constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
-			multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
-			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
-		} else
-		{
-			btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
-		}
-
-		constraintRow.m_jacAindex = data.m_jacobians.size();
-		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
-		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
-		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
-		for (int i=0;i<ndofA;i++)
-			data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
-		
-		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
-		multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
-	} 
-
-	if (multiBodyB)
-	{
-		const int ndofB  = multiBodyB->getNumLinks() + 6;
-
-		constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
-		if (constraintRow.m_deltaVelBindex <0)
-		{
-			constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
-			multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
-			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
-		}
-
-		constraintRow.m_jacBindex = data.m_jacobians.size();
-		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
-
-		for (int i=0;i<ndofB;i++)
-			data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
-
-		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
-		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
-		multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
-	} 
-	{
-						
-		btVector3 vec;
-		btScalar denom0 = 0.f;
-		btScalar denom1 = 0.f;
-		btScalar* jacB = 0;
-		btScalar* jacA = 0;
-		btScalar* lambdaA =0;
-		btScalar* lambdaB =0;
-		int ndofA  = 0;
-		if (multiBodyA)
-		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
-			jacA = &data.m_jacobians[constraintRow.m_jacAindex];
-			lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
-			for (int i = 0; i < ndofA; ++i)
-			{
-				btScalar j = jacA[i] ;
-				btScalar l =lambdaA[i];
-				denom0 += j*l;
-			}
-		} 
-		if (multiBodyB)
-		{
-			const int ndofB  = multiBodyB->getNumLinks() + 6;
-			jacB = &data.m_jacobians[constraintRow.m_jacBindex];
-			lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
-			for (int i = 0; i < ndofB; ++i)
-			{
-				btScalar j = jacB[i] ;
-				btScalar l =lambdaB[i];
-				denom1 += j*l;
-			}
-
-		} 
-
-		 if (multiBodyA && (multiBodyA==multiBodyB))
-		 {
-            // ndof1 == ndof2 in this case
-            for (int i = 0; i < ndofA; ++i) 
-			{
-                denom1 += jacB[i] * lambdaA[i];
-                denom1 += jacA[i] * lambdaB[i];
-            }
-        }
-
-		 btScalar d = denom0+denom1;
-		 if (btFabs(d)>SIMD_EPSILON)
-		 {
-			 
-			 constraintRow.m_jacDiagABInv = 1.f/(d);
-		 } else
-		 {
-			constraintRow.m_jacDiagABInv  = 1.f;
-		 }
-		
-	}
-
-	
-	//compute rhs and remaining constraintRow fields
-
-	
-
-
-	btScalar rel_vel = 0.f;
-	int ndofA  = 0;
-	int ndofB  = 0;
-	{
-
-		btVector3 vel1,vel2;
-		if (multiBodyA)
-		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
-			btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
-			for (int i = 0; i < ndofA ; ++i) 
-				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
-		} 
-		if (multiBodyB)
-		{
-			ndofB  = multiBodyB->getNumLinks() + 6;
-			btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
-			for (int i = 0; i < ndofB ; ++i) 
-				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-
-		}
-
-		constraintRow.m_friction = 0.f;
-
-		constraintRow.m_appliedImpulse = 0.f;
-		constraintRow.m_appliedPushImpulse = 0.f;
-		
-		btScalar	velocityError =  desiredVelocity - rel_vel;// * damping;
-
-		btScalar erp = infoGlobal.m_erp2;
-
-		btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
-
-		if (!infoGlobal.m_splitImpulse)
-		{
-			//combine position and velocity into rhs
-			constraintRow.m_rhs = velocityImpulse;
-			constraintRow.m_rhsPenetration = 0.f;
-
-		} else
-		{
-			//split position and velocity into rhs and m_rhsPenetration
-			constraintRow.m_rhs = velocityImpulse;
-			constraintRow.m_rhsPenetration = 0.f;
-		}
-
-
-		constraintRow.m_cfm = 0.f;
-		constraintRow.m_lowerLimit = lowerLimit;
-		constraintRow.m_upperLimit = upperLimit;
-
-	}
-	return rel_vel;
-}
-
-
 void	btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
 {
 	for (int i = 0; i < ndof; ++i) 
 		data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
 }
 
-
+btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstraint& solverConstraint, 
-void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, 
 															btMultiBodyJacobianData& data,
+															btScalar* jacOrgA, btScalar* jacOrgB,
 															const btVector3& contactNormalOnB,
 															const btVector3& posAworld, const btVector3& posBworld, 
-																 btScalar position,
+															btScalar posError,
 															const btContactSolverInfo& infoGlobal,
-																 btScalar& relaxation,
+															btScalar lowerLimit, btScalar upperLimit,
+															btScalar relaxation,
 															bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
-			
-	
-	btVector3 rel_pos1 = posAworld;
-	btVector3 rel_pos2 = posBworld;
-
 	solverConstraint.m_multiBodyA = m_bodyA;
 	solverConstraint.m_multiBodyB = m_bodyB;
 	solverConstraint.m_linkA = m_linkA;
 	solverConstraint.m_linkB = m_linkB;	
 
-
 	btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
 	btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
 
-	const btVector3& pos1 = posAworld;
-	const btVector3& pos2 = posBworld;
-
 	btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
 	btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
 
 	btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
 	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
 
+	btVector3 rel_pos1, rel_pos2;				//these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
 	if (bodyA)
-		rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
+		rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
 	if (bodyB)
-		rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
+		rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
-
-	relaxation = 1.f;
 
 	if (multiBodyA)
 	{
-		const int ndofA  = multiBodyA->getNumLinks() + 6;
+		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 
 		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 
@@ -271,16 +93,37 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 			btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
 		}
 
+		//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
+		//resize..
 		solverConstraint.m_jacAindex = data.m_jacobians.size();
 		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
-		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
+		//copy/determine
-		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+		if(jacOrgA)
-
+		{
+			for (int i=0;i<ndofA;i++)
+				data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
+		}
+		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);
+			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)
+		//resize..
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);		//=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
+		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);
+		else
 			multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
-	} else
+	}
+	else if(rb0)
 	{
 		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
 		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
@@ -290,7 +133,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 
 	if (multiBodyB)
 	{
-		const int ndofB  = multiBodyB->getNumLinks() + 6;
+		const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 
 		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
 		if (solverConstraint.m_deltaVelBindex <0)
@@ -300,22 +143,43 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
 		}
 
+		//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
+		//resize..
 		solverConstraint.m_jacBindex = data.m_jacobians.size();
-
 		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
+		//copy/determine..
+		if(jacOrgB)
+		{
+			for (int i=0;i<ndofB;i++)
+				data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
+		}
+		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);
+			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)
+		//resize..
 		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
 		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);
+		else
+			multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
 
-		multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+	}
-		multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
+	else if(rb1)
-	} else
 	{
 		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);		
 		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
 		solverConstraint.m_contactNormal2 = -contactNormalOnB;
 	}
-
 	{
 						
 		btVector3 vec;
@@ -323,121 +187,103 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 		btScalar denom1 = 0.f;
 		btScalar* jacB = 0;
 		btScalar* jacA = 0;
-		btScalar* lambdaA =0;
+		btScalar* deltaVelA = 0;
-		btScalar* lambdaB =0;
+		btScalar* deltaVelB = 0;
 		int ndofA  = 0;
+		//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
 		if (multiBodyA)
 		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
+			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 			jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
-			lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+			deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
 			{
 				btScalar j = jacA[i] ;
-				btScalar l =lambdaA[i];
+				btScalar l = deltaVelA[i];
 				denom0 += j*l;
 			}
-		} else
+		}
-		{
+		else if(rb0)
-			if (rb0)
 		{
 			vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
 			denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
 		}
-		}
+		//
 		if (multiBodyB)
 		{
-			const int ndofB  = multiBodyB->getNumLinks() + 6;
+			const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 			jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
-			lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+			deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
 			{
 				btScalar j = jacB[i] ;
-				btScalar l =lambdaB[i];
+				btScalar l = deltaVelB[i];
 				denom1 += j*l;
 			}
 
-		} else
+		}
-		{
+		else if(rb1)
-			if (rb1)
 		{
 			vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
 			denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
 		}
-		}
+		//determine the "effective mass" of the constrained multibodyB with respect to this 1D constraint (i.e. 1/A[i,i])
-
 		if (multiBodyA && (multiBodyA==multiBodyB))
 		{
 			// ndof1 == ndof2 in this case
 			for (int i = 0; i < ndofA; ++i) 
 			{
-                denom1 += jacB[i] * lambdaA[i];
+				denom1 += jacB[i] * deltaVelA[i];
-                denom1 += jacA[i] * lambdaB[i];
+				denom1 += jacA[i] * deltaVelB[i];
 			}
 		}
-
+		//
 		btScalar d = denom0+denom1;
 		if (btFabs(d)>SIMD_EPSILON)
 		{
 			 
 			solverConstraint.m_jacDiagABInv = relaxation/(d);
-		 } else
+		}
+		else
 		{
 			solverConstraint.m_jacDiagABInv  = 1.f;
 		}		
-		
 	}
 
 	
 	//compute rhs and remaining solverConstraint fields
-
+	btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
-	
-
-	btScalar restitution = 0.f;
-	btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop;
 
 	btScalar rel_vel = 0.f;
 	int ndofA  = 0;
 	int ndofB  = 0;
 	{
-
 		btVector3 vel1,vel2;
 		if (multiBodyA)
 		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
+			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 			btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA ; ++i) 
 				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
-		} else
+		}
-		{
+		else if(rb0)
-			if (rb0)
 		{
 			rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
 		}
-		}
 		if (multiBodyB)
 		{
-			ndofB  = multiBodyB->getNumLinks() + 6;
+			ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 			btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB ; ++i) 
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
 
-		} else
+		}
-		{
+		else if(rb1)
-			if (rb1)
 		{
 			rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
 		}
-		}
 
 		solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
-
-				
-		restitution =  restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
-		if (restitution <= btScalar(0.))
-		{
-			restitution = 0.f;
-		};
 	}
 
 
@@ -474,17 +320,14 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 		}
 	} else
 	*/
-	{
-		solverConstraint.m_appliedImpulse = 0.f;
-	}
 
+	solverConstraint.m_appliedImpulse = 0.f;
 	solverConstraint.m_appliedPushImpulse = 0.f;
 
 	{	
 
-
 		btScalar positionalError = 0.f;
-		btScalar	velocityError = restitution - rel_vel;// * damping;
+		btScalar	velocityError = desiredVelocity - rel_vel;// * damping;
 					
 
 		btScalar erp = infoGlobal.m_erp2;
@@ -493,15 +336,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 			erp = infoGlobal.m_erp;
 		}
 
-		if (penetration>0)
-		{
-			positionalError = 0;
-			velocityError = -penetration / infoGlobal.m_timeStep;
-
-		} else
-		{
 		positionalError = -penetration * erp/infoGlobal.m_timeStep;
-		}
 
 		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
 		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
@@ -520,8 +355,10 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 		}
 
 		solverConstraint.m_cfm = 0.f;
-		solverConstraint.m_lowerLimit = -m_maxAppliedImpulse;
+		solverConstraint.m_lowerLimit = lowerLimit;
-		solverConstraint.m_upperLimit = m_maxAppliedImpulse;
+		solverConstraint.m_upperLimit = upperLimit;
 	}
 
+	return rel_vel;
+
 }

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -28,8 +28,8 @@ struct btSolverInfo;
 struct btMultiBodyJacobianData
 {
 	btAlignedObjectArray<btScalar>		m_jacobians;
-	btAlignedObjectArray<btScalar>		m_deltaVelocitiesUnitImpulse;
+	btAlignedObjectArray<btScalar>		m_deltaVelocitiesUnitImpulse;	//holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
-	btAlignedObjectArray<btScalar>		m_deltaVelocities;
+	btAlignedObjectArray<btScalar>		m_deltaVelocities;				//holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
 	btAlignedObjectArray<btScalar>		scratch_r;
 	btAlignedObjectArray<btVector3>		scratch_v;
 	btAlignedObjectArray<btMatrix3x3>	scratch_m;
@@ -48,10 +48,10 @@ protected:
     int				m_linkA;
     int				m_linkB;
 
-    int				m_num_rows;
+    int				m_numRows;
-    int				m_jac_size_A;
+    int				m_jacSizeA;
-    int				m_jac_size_both;
+    int				m_jacSizeBoth;
-    int				m_pos_offset;
+    int				m_posOffset;
 
 	bool			m_isUnilateral;
 
@@ -66,22 +66,16 @@ protected:
 
 	void	applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
 
-	void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, 
+	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, 
 																btMultiBodyJacobianData& data,
+																btScalar* jacOrgA, btScalar* jacOrgB,
 																const btVector3& contactNormalOnB,
 																const btVector3& posAworld, const btVector3& posBworld, 
-																 btScalar position,
+																btScalar posError,
 																const btContactSolverInfo& infoGlobal,
-																 btScalar& relaxation,
+																btScalar lowerLimit, btScalar upperLimit,
-																 bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
+																btScalar relaxation = 1.f,
-
+																bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
-		btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
-														btMultiBodyJacobianData& data,
-														btScalar* jacOrgA,btScalar* jacOrgB,
-														const btContactSolverInfo& infoGlobal,
-														btScalar desiredVelocity,
-														btScalar lowerLimit,
-														btScalar upperLimit);
 
 public:
 
@@ -99,7 +93,7 @@ public:
 
 	int	getNumRows() const
 	{
-		return m_num_rows;
+		return m_numRows;
 	}
 
 	btMultiBody*	getMultiBodyA()
@@ -116,12 +110,12 @@ public:
     // NOTE: ignored position for friction rows.
     btScalar getPosition(int row) const 
 	{ 
-		return m_data[m_pos_offset + row]; 
+		return m_data[m_posOffset + row]; 
 	}
 
     void setPosition(int row, btScalar pos) 
 	{ 
-		m_data[m_pos_offset + row] = pos; 
+		m_data[m_posOffset + row] = pos; 
 	}
 
 	
@@ -135,19 +129,19 @@ public:
     // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
     btScalar* jacobianA(int row) 
 	{ 
-		return &m_data[m_num_rows + row * m_jac_size_both]; 
+		return &m_data[m_numRows + row * m_jacSizeBoth]; 
 	}
     const btScalar* jacobianA(int row) const 
 	{ 
-		return &m_data[m_num_rows + (row * m_jac_size_both)]; 
+		return &m_data[m_numRows + (row * m_jacSizeBoth)]; 
 	}
     btScalar* jacobianB(int row) 
 	{ 
-		return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; 
+		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; 
 	}
     const btScalar* jacobianB(int row) const 
 	{ 
-		return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; 
+		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; 
 	}
 
 	btScalar	getMaxAppliedImpulse() const

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -36,6 +36,8 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 		//if (iteration < constraint.m_overrideNumSolverIterations)
 			//resolveSingleConstraintRowGenericMultiBody(constraint);
 		resolveSingleConstraintRowGeneric(constraint);
+		if(constraint.m_multiBodyA) constraint.m_multiBodyA->__posUpdated = false;
+		if(constraint.m_multiBodyB) constraint.m_multiBodyB->__posUpdated = false;
 	}
 
 	//solve featherstone normal contact
@@ -44,6 +46,9 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 		btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
 		if (iteration < infoGlobal.m_numIterations)
 			resolveSingleConstraintRowGeneric(constraint);
+
+		if(constraint.m_multiBodyA) constraint.m_multiBodyA->__posUpdated = false;
+		if(constraint.m_multiBodyB) constraint.m_multiBodyB->__posUpdated = false;
 	}
 	
 	//solve featherstone frictional contact
@@ -60,6 +65,9 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 				frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
 				frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
 				resolveSingleConstraintRowGeneric(frictionConstraint);
+
+				if(frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->__posUpdated = false;
+				if(frictionConstraint.m_multiBodyB) frictionConstraint.m_multiBodyB->__posUpdated = false;
 			}
 		}
 	}
@@ -108,10 +116,10 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 
 	if (c.m_multiBodyA)
 	{
-		ndofA  = c.m_multiBodyA->getNumLinks() + 6;
+		ndofA  = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 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
+	} else if(c.m_solverBodyIdA >= 0)
 	{
 		bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
 		deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
@@ -119,10 +127,10 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 
 	if (c.m_multiBodyB)
 	{
-		ndofB = c.m_multiBodyB->getNumLinks() + 6;
+		ndofB  = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 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
+	} else if(c.m_solverBodyIdB >= 0)
 	{
 		bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
 		deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
@@ -151,8 +159,13 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 	if (c.m_multiBodyA)
 	{
 		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
+		//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->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
+		else
 			c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
-	} else
+	} else if(c.m_solverBodyIdA >= 0)
 	{
 		bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
 
@@ -160,8 +173,13 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 	if (c.m_multiBodyB)
 	{
 		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
+		//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->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
+		else
 			c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
-	} else
+	} else if(c.m_solverBodyIdB >= 0)
 	{
 		bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
 	}
@@ -180,14 +198,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(con
 
 	if (c.m_multiBodyA)
 	{
-		ndofA  = c.m_multiBodyA->getNumLinks() + 6;
+		ndofA  = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 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->getNumLinks() + 6;
+		ndofB  = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 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];
 	}
@@ -257,7 +275,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 	if (multiBodyA)
 	{
-		const int ndofA  = multiBodyA->getNumLinks() + 6;
+		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 
 		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 
@@ -277,8 +295,14 @@ 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);
+		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);
+		else
 			multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
 	} else
 	{
@@ -290,7 +314,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 	if (multiBodyB)
 	{
-		const int ndofB  = multiBodyB->getNumLinks() + 6;
+		const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 
 		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
 		if (solverConstraint.m_deltaVelBindex <0)
@@ -306,7 +330,13 @@ 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);
+		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);
 	} else
 	{
@@ -328,7 +358,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		int ndofA  = 0;
 		if (multiBodyA)
 		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
+			ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 			jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 			lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
@@ -347,7 +377,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 		if (multiBodyB)
 		{
-			const int ndofB  = multiBodyB->getNumLinks() + 6;
+			const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 			jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
 			lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
@@ -404,7 +434,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		btVector3 vel1,vel2;
 		if (multiBodyA)
 		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
+			ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 			btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA ; ++i) 
 				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
@@ -417,7 +447,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 		if (multiBodyB)
 		{
-			ndofB  = multiBodyB->getNumLinks() + 6;
+			ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 			btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB ; ++i) 
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
@@ -432,12 +462,14 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 		solverConstraint.m_friction = cp.m_combinedFriction;
 
-				
+		if(!isFriction)
+		{
 			restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);	
 			if (restitution <= btScalar(0.))
 			{
 				restitution = 0.f;
-		};
+			}
+		}
 	}
 
 
@@ -452,6 +484,9 @@ 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);
+				else
 					multiBodyA->applyDeltaVee(deltaV,impulse);
 				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
 			} else
@@ -463,6 +498,9 @@ 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);
+				else
 					multiBodyB->applyDeltaVee(deltaV,impulse);
 				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
 			} else
@@ -480,10 +518,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 	{
 
-
 		btScalar positionalError = 0.f;
-		btScalar	velocityError = restitution - rel_vel;// * damping;
+		btScalar velocityError = restitution - rel_vel;// * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
-					
 
 		btScalar erp = infoGlobal.m_erp2;
 		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
@@ -494,7 +530,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		if (penetration>0)
 		{
 			positionalError = 0;
-			velocityError = -penetration / infoGlobal.m_timeStep;
+			velocityError -= penetration / infoGlobal.m_timeStep;
 
 		} else
 		{
@@ -504,6 +540,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
 		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
 
+		if(!isFriction)
+		{
 			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 			{
 				//combine position and velocity into rhs
@@ -517,10 +555,19 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 				solverConstraint.m_rhsPenetration = penetrationImpulse;
 			}
 
-		solverConstraint.m_cfm = 0.f;
 			solverConstraint.m_lowerLimit = 0;
 			solverConstraint.m_upperLimit = 1e10f;
 		}
+		else
+		{
+			solverConstraint.m_rhs = velocityImpulse;
+			solverConstraint.m_rhsPenetration = 0.f;
+			solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
+			solverConstraint.m_upperLimit = solverConstraint.m_friction;
+		}
+
+		solverConstraint.m_cfm = 0.f;			//why not use cfmSlip?
+	}
 
 }
 
@@ -702,6 +749,10 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 				{
 					btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
 
+					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
+
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
@@ -709,10 +760,6 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 						addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
 					}
 
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
-
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
 					{
 						cp.m_lateralFrictionInitialized = true;

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -366,6 +366,8 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 		btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
 		btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;			
 
+		//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
+	
 		m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
 		m_bodies.resize(0);
 		m_manifolds.resize(0);
@@ -392,9 +394,6 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
 	delete m_solverMultiBodyIslandCallback;
 }
 
-
-
-
 void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
 
@@ -451,7 +450,8 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 
 			if (!isSleeping)
 			{
-				scratch_r.resize(bod->getNumLinks()+1);
+				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+				scratch_r.resize(bod->getNumLinks()+1);			//multidof? ("Y"s use it and it is used to store qdd)
 				scratch_v.resize(bod->getNumLinks()+1);
 				scratch_m.resize(bod->getNumLinks()+1);
 
@@ -463,6 +463,169 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 					bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
 				}
 
+				bool doNotUpdatePos = false;
+
+				if(bod->isMultiDof())
+				{
+					if(!bod->isUsingRK4Integration())
+						bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
+					else
+					{						
+						//
+						int numDofs = bod->getNumDofs() + 6;
+						int numPosVars = bod->getNumPosVars() + 7;
+						btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
+						//convenience
+						btScalar *pMem = &scratch_r2[0];
+						btScalar *scratch_q0 = pMem; pMem += numPosVars;
+						btScalar *scratch_qx = pMem; pMem += numPosVars;
+						btScalar *scratch_qd0 = pMem; pMem += numDofs;
+						btScalar *scratch_qd1 = pMem; pMem += numDofs;
+						btScalar *scratch_qd2 = pMem; pMem += numDofs;
+						btScalar *scratch_qd3 = pMem; pMem += numDofs;
+						btScalar *scratch_qdd0 = pMem; pMem += numDofs;
+						btScalar *scratch_qdd1 = pMem; pMem += numDofs;
+						btScalar *scratch_qdd2 = pMem; pMem += numDofs;
+						btScalar *scratch_qdd3 = pMem; pMem += numDofs;
+						btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
+
+						/////						
+						//copy q0 to scratch_q0 and qd0 to scratch_qd0
+						scratch_q0[0] = bod->getWorldToBaseRot().x();
+						scratch_q0[1] = bod->getWorldToBaseRot().y();
+						scratch_q0[2] = bod->getWorldToBaseRot().z();
+						scratch_q0[3] = bod->getWorldToBaseRot().w();
+						scratch_q0[4] = bod->getBasePos().x();
+						scratch_q0[5] = bod->getBasePos().y();
+						scratch_q0[6] = bod->getBasePos().z();
+						//
+						for(int link = 0; link < bod->getNumLinks(); ++link)
+						{
+							for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
+								scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];							
+						}
+						//
+						for(int dof = 0; dof < numDofs; ++dof)								
+							scratch_qd0[dof] = bod->getVelocityVector()[dof];
+						////
+						struct
+						{
+						    btMultiBody *bod;
+                            btScalar *scratch_qx, *scratch_q0;
+
+						    void operator()()
+						    {
+						        for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
+                                    scratch_qx[dof] = scratch_q0[dof];
+						    }
+						} pResetQx = {bod, scratch_qx, scratch_q0};
+						//
+						struct
+						{
+						    void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
+						    {
+						        for(int i = 0; i < size; ++i)
+                                    pVal[i] = pCurVal[i] + dt * pDer[i];
+						    }
+
+						} pEulerIntegrate;
+						//
+						struct
+                        {
+                            void operator()(btMultiBody *pBody, const btScalar *pData)
+                            {
+                                btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
+
+                                for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
+                                    pVel[i] = pData[i];
+
+                            }
+                        } pCopyToVelocityVector;
+						//
+                        struct
+						{
+						    void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
+						    {
+						        for(int i = 0; i < size; ++i)
+                                    pDst[i] = pSrc[start + i];
+						    }
+						} pCopy;
+						//
+
+						btScalar h = solverInfo.m_timeStep;
+						#define output &scratch_r[bod->getNumDofs()]
+						//calc qdd0 from: q0 & qd0	
+						bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
+						pCopy(output, scratch_qdd0, 0, numDofs);
+						//calc q1 = q0 + h/2 * qd0
+						pResetQx();
+						bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
+						//calc qd1 = qd0 + h/2 * qdd0
+						pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
+						//
+						//calc qdd1 from: q1 & qd1
+						pCopyToVelocityVector(bod, scratch_qd1);
+						bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
+						pCopy(output, scratch_qdd1, 0, numDofs);
+						//calc q2 = q0 + h/2 * qd1
+						pResetQx();
+						bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
+						//calc qd2 = qd0 + h/2 * qdd1
+						pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
+						//
+						//calc qdd2 from: q2 & qd2
+						pCopyToVelocityVector(bod, scratch_qd2);
+						bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
+						pCopy(output, scratch_qdd2, 0, numDofs);
+						//calc q3 = q0 + h * qd2
+						pResetQx();
+						bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
+						//calc qd3 = qd0 + h * qdd2
+						pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
+						//
+						//calc qdd3 from: q3 & qd3
+						pCopyToVelocityVector(bod, scratch_qd3);
+						bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
+						pCopy(output, scratch_qdd3, 0, numDofs);
+
+						//
+						//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
+						//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)						
+						btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
+						btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
+						for(int i = 0; i < numDofs; ++i)
+						{
+							delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
+							delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);							
+							//delta_q[i] = h*scratch_qd0[i];
+							//delta_qd[i] = h*scratch_qdd0[i];
+						}
+						//
+						pCopyToVelocityVector(bod, scratch_qd0);
+						bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);						
+						//
+						if(!doNotUpdatePos)
+						{
+							btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
+							pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
+
+							for(int i = 0; i < numDofs; ++i)
+								pRealBuf[i] = delta_q[i];
+
+							//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
+							bod->__posUpdated = true;							
+						}
+
+						//ugly hack which resets the cached data to t0 (needed for constraint solver)
+						{
+							for(int link = 0; link < bod->getNumLinks(); ++link)
+								bod->getLink(link).updateCacheMultiDof();
+							bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m);
+						}
+						
+					}
+				}
+				else
 					bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
 			}
 		}
@@ -503,14 +666,26 @@ void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 			{
 				int nLinks = bod->getNumLinks();
 
-				///base + num links
+				///base + num m_links
 				world_to_local.resize(nLinks+1);
 				local_origin.resize(nLinks+1);
 
+				if(bod->isMultiDof())
+				{
+					if(!bod->__posUpdated)
+						bod->stepPositionsMultiDof(timeStep);
+					else
+					{
+						btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
+						pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
+
+						bod->stepPositionsMultiDof(1, 0, pRealBuf);
+						bod->__posUpdated = false;
+					}
+				}
+				else
 					bod->stepPositions(timeStep);			
 
-	 
-
 				world_to_local[0] = bod->getWorldToBaseRot();
 				local_origin[0] = bod->getBasePos();
 

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -22,6 +22,7 @@ subject to the following restrictions:
 
 
 btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
+	//:btMultiBodyConstraint(body,0,link,-1,2,true),
 	:btMultiBodyConstraint(body,body,link,link,2,true),
 	m_lowerBound(lower),
 	m_upperBound(upper)
@@ -32,11 +33,14 @@ btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* bo
     // note: we rely on the fact that data.m_jacobians are
     // always initialized to zero by the Constraint ctor
 
-    // row 0: the lower bound
+	unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link);
-    jacobianA(0)[6 + link] = 1;
 
+	// row 0: the lower bound
+	jacobianA(0)[offset] = 1;
 	// row 1: the upper bound
-    jacobianB(1)[6 + link] = -1;
+	//jacobianA(1)[offset] = -1;
+
+	jacobianB(1)[offset] = -1;
 }
 btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
 {
@@ -44,6 +48,8 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
 
 int btMultiBodyJointLimitConstraint::getIslandIdA() const
 {
+	if(m_bodyA)
+	{
 		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
 		if (col)
 			return col->getIslandTag();
@@ -52,11 +58,14 @@ int btMultiBodyJointLimitConstraint::getIslandIdA() const
 			if (m_bodyA->getLink(i).m_collider)
 				return m_bodyA->getLink(i).m_collider->getIslandTag();
 		}
+	}
 	return -1;
 }
 
 int btMultiBodyJointLimitConstraint::getIslandIdB() const
 {
+	if(m_bodyB)
+	{
 		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 		if (col)
 			return col->getIslandTag();
@@ -67,6 +76,7 @@ int btMultiBodyJointLimitConstraint::getIslandIdB() const
 			if (col)
 				return col->getIslandTag();
 		}
+	}
 	return -1;
 }
 
@@ -79,7 +89,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
     // directions were set in the ctor and never change.
     
     // row 0: the lower bound
-    setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);
+    setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);			//multidof: this is joint-type dependent
 
     // row 1: the upper bound
     setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
@@ -89,8 +99,10 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
 		constraintRow.m_multiBodyA = m_bodyA;
 		constraintRow.m_multiBodyB = m_bodyB;
+		const btScalar posError = 0;						//why assume it's zero?
+		const btVector3 dummy(0, 0, 0);
 		
-		btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,-m_maxAppliedImpulse,m_maxAppliedImpulse);
+		btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
 		{
 			btScalar penetration = getPosition(row);
 			btScalar positionalError = 0.f;

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -21,10 +21,13 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
 
-btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
+btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
+	//:btMultiBodyConstraint(body,0,link,-1,1,true),
 	:btMultiBodyConstraint(body,body,link,link,1,true),
 	m_desiredVelocity(desiredVelocity)	
 {
+	btAssert(linkDoF < body->getLink(link).m_dofCount);
+
 	m_maxAppliedImpulse = maxMotorImpulse;
 	// the data.m_jacobians never change, so may as well
     // initialize them here
@@ -32,8 +35,11 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
     // note: we rely on the fact that data.m_jacobians are
     // always initialized to zero by the Constraint ctor
 
+    unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link);
+
 	// row 0: the lower bound
-    jacobianA(0)[6 + link] = 1;
+	// row 0: the lower bound
+    jacobianA(0)[offset] = 1;
 }
 btMultiBodyJointMotor::~btMultiBodyJointMotor()
 {
@@ -76,13 +82,15 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
     // directions were set in the ctor and never change.
     
   
+	const btScalar posError = 0;
+	const btVector3 dummy(0, 0, 0);
 
 	for (int row=0;row<getNumRows();row++)
 	{
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
 		
 		btScalar penetration = 0;
-		fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,m_desiredVelocity,-m_maxAppliedImpulse,m_maxAppliedImpulse);
+		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
 	}
 
 }

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h

@@ -30,7 +30,7 @@ protected:
 
 public:
 
-	btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
+	btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
 	virtual ~btMultiBodyJointMotor();
 
 	virtual int getIslandIdA() const;

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -24,6 +24,327 @@ enum	btMultiBodyLinkFlags
 {
 	BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
 };
+
+//#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+#define TEST_SPATIAL_ALGEBRA_LAYER
+
+//
+// Various spatial helper functions
+//
+
+namespace {
+
+#ifdef TEST_SPATIAL_ALGEBRA_LAYER
+
+	struct btSpatialForceVector
+	{	
+		btVector3 m_topVec, m_bottomVec;	
+		//
+		btSpatialForceVector() { setZero(); }
+		btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {}
+		btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
+		{
+			setValue(ax, ay, az, lx, ly, lz);
+		}
+		//
+		void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; }
+		void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
+		{
+			m_bottomVec.setValue(ax, ay, az); m_topVec.setValue(lx, ly, lz);
+		}
+		//
+		void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
+		void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
+		{
+			m_bottomVec[0] += ax; m_bottomVec[1] += ay; m_bottomVec[2] += az;
+			m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz;			
+		}
+		//
+		const btVector3 & getLinear()  const { return m_topVec; }
+		const btVector3 & getAngular() const { return m_bottomVec; }
+		//
+		void setLinear(const btVector3 &linear) { m_topVec = linear; }
+		void setAngular(const btVector3 &angular) { m_bottomVec = angular; }
+		//
+		void addAngular(const btVector3 &angular) { m_bottomVec += angular; }
+		void addLinear(const btVector3 &linear) { m_topVec += linear; }
+		//
+		void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
+		//
+		btSpatialForceVector & operator += (const btSpatialForceVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
+		btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
+		btSpatialForceVector operator - (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); }
+		btSpatialForceVector operator + (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); }
+		btSpatialForceVector operator - () const { return btSpatialForceVector(-m_bottomVec, -m_topVec); }
+		btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); }		
+		//btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; }
+	};
+
+	struct btSpatialMotionVector
+	{
+		btVector3 m_topVec, m_bottomVec;
+		//
+		btSpatialMotionVector() { setZero(); }
+		btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {}		
+		//
+		void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; }
+		void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
+		{
+			m_topVec.setValue(ax, ay, az); m_bottomVec.setValue(lx, ly, lz);
+		}
+		//
+		void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
+		void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
+		{
+			m_topVec[0] += ax; m_topVec[1] += ay; m_topVec[2] += az;
+			m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz;			
+		}
+		//	
+		const btVector3 & getAngular() const { return m_topVec; }
+		const btVector3 & getLinear() const { return m_bottomVec; }
+		//
+		void setAngular(const btVector3 &angular) { m_topVec = angular; }
+		void setLinear(const btVector3 &linear) { m_bottomVec = linear; }
+		//
+		void addAngular(const btVector3 &angular) { m_topVec += angular; }
+		void addLinear(const btVector3 &linear) { m_bottomVec += linear; }
+		//
+		void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
+		//
+		btScalar dot(const btSpatialForceVector &b) const
+		{
+			return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec);
+		}
+		//
+		template<typename SpatialVectorType>
+		void cross(const SpatialVectorType &b, SpatialVectorType &out) const
+		{
+			out.m_topVec = m_topVec.cross(b.m_topVec);
+			out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
+		}
+		template<typename SpatialVectorType>
+		SpatialVectorType cross(const SpatialVectorType &b) const
+		{
+			SpatialVectorType out;
+			out.m_topVec = m_topVec.cross(b.m_topVec);
+			out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
+			return out;
+		}
+		//
+		btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
+		btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
+		btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; }
+		btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); }
+		btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); }
+		btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); }
+		btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); }
+	};
+
+	struct btSymmetricSpatialDyad
+	{
+		btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat;
+		//		
+		btSymmetricSpatialDyad() { setIdentity(); }
+		btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); }			
+		//
+		void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
+		{
+			m_topLeftMat = topLeftMat;
+			m_topRightMat = topRightMat;
+			m_bottomLeftMat = bottomLeftMat;
+		}
+		//
+		void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
+		{
+			m_topLeftMat += topLeftMat;
+			m_topRightMat += topRightMat;
+			m_bottomLeftMat += bottomLeftMat;
+		}
+		//
+		void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity();  }
+		//
+		btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat)
+		{
+			m_topLeftMat -= mat.m_topLeftMat;
+			m_topRightMat -= mat.m_topRightMat;
+			m_bottomLeftMat -= mat.m_bottomLeftMat;
+			return *this; 
+		}
+		//
+		btSpatialForceVector operator * (const btSpatialMotionVector &vec)
+		{
+			return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec);
+		}
+	};
+
+	struct btSpatialTransformationMatrix
+	{
+		btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat;
+		btVector3 m_trnVec;
+		//
+		enum eOutputOperation
+		{
+			None = 0,
+			Add = 1,
+			Subtract = 2
+		};
+		//
+		template<typename SpatialVectorType>
+		void transform(	const SpatialVectorType &inVec,
+                        SpatialVectorType &outVec,
+						eOutputOperation outOp = None)
+		{
+			if(outOp == None)
+			{
+				outVec.m_topVec = m_rotMat * inVec.m_topVec;
+				outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
+			}
+			else if(outOp == Add)
+			{
+				outVec.m_topVec += m_rotMat * inVec.m_topVec;
+				outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
+			}
+			else if(outOp == Subtract)
+			{
+				outVec.m_topVec -= m_rotMat * inVec.m_topVec;
+				outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
+			}
+			
+		}
+
+		template<typename SpatialVectorType>
+		void transformRotationOnly(	const SpatialVectorType &inVec,
+									SpatialVectorType &outVec,
+									eOutputOperation outOp = None)
+		{
+			if(outOp == None)
+			{
+				outVec.m_topVec = m_rotMat * inVec.m_topVec;
+				outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec;
+			}
+			else if(outOp == Add)
+			{
+				outVec.m_topVec += m_rotMat * inVec.m_topVec;
+				outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec;
+			}
+			else if(outOp == Subtract)
+			{
+				outVec.m_topVec -= m_rotMat * inVec.m_topVec;
+				outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec;
+			}
+			
+		}
+
+		template<typename SpatialVectorType>
+		void transformInverse(	const SpatialVectorType &inVec,
+								SpatialVectorType &outVec,
+								eOutputOperation outOp = None)
+		{
+			if(outOp == None)
+			{
+				outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
+				outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
+			}
+			else if(outOp == Add)
+			{
+				outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
+				outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
+			}
+			else if(outOp == Subtract)
+			{
+				outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
+				outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
+			}			
+		}
+
+		template<typename SpatialVectorType>
+		void transformInverseRotationOnly(	const SpatialVectorType &inVec,
+											SpatialVectorType &outVec,
+											eOutputOperation outOp = None)
+		{
+			if(outOp == None)
+			{
+				outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
+				outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec;
+			}
+			else if(outOp == Add)
+			{
+				outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
+				outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec;
+			}
+			else if(outOp == Subtract)
+			{
+				outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
+				outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec;
+			}
+			
+		}
+
+		void transformInverse(	const btSymmetricSpatialDyad &inMat,
+								btSymmetricSpatialDyad &outMat,
+								eOutputOperation outOp = None)
+		{
+			const btMatrix3x3 r_cross(	0, -m_trnVec[2], m_trnVec[1],
+									m_trnVec[2], 0, -m_trnVec[0],
+									-m_trnVec[1], m_trnVec[0], 0);
+
+
+			if(outOp == None)
+			{
+				outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
+				outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
+				outMat.m_bottomLeftMat = m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
+			}
+			else if(outOp == Add)
+			{
+				outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
+				outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
+				outMat.m_bottomLeftMat += m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
+			}
+			else if(outOp == Subtract)
+			{
+				outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
+				outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
+				outMat.m_bottomLeftMat -= m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
+			}
+		}
+
+		template<typename SpatialVectorType>
+		SpatialVectorType operator * (const SpatialVectorType &vec)
+		{
+			SpatialVectorType out;
+			transform(vec, out);
+			return out;
+		}
+	};
+
+	template<typename SpatialVectorType>
+	void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
+	{
+		//output op maybe?
+
+		out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec);
+		out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec);
+		out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec);
+		//maybe simple a*spatTranspose(a) would be nicer?
+	}
+
+	template<typename SpatialVectorType>
+	btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b)
+	{
+		btSymmetricSpatialDyad out;
+
+		out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec);
+		out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec);
+		out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec);
+
+		return out;
+		//maybe simple a*spatTranspose(a) would be nicer?
+	}
+
+#endif
+}
+
 //
 // Link struct
 //
@@ -33,75 +354,159 @@ struct btMultibodyLink
 
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    btScalar joint_pos;    // qi
+    btScalar m_mass;         // mass of link
+    btVector3 m_inertia;   // inertia of link (local frame; diagonal)
 
-    btScalar mass;         // mass of link
+    int m_parent;         // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
-    btVector3 inertia;   // inertia of link (local frame; diagonal)
 
-    int parent;         // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
+    btQuaternion m_zeroRotParentToThis;    // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
 
-    btQuaternion zero_rot_parent_to_this;    // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
+    btVector3 m_dVector;   // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only.
 
-    // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
+    // m_eVector is constant, but depends on the joint type
-    // for prismatic: axis_top = zero;
-    //                axis_bottom = unit vector along the joint axis.
-    // for revolute: axis_top = unit vector along the rotation axis (u);
-    //               axis_bottom = u cross d_vector.
-    btVector3 axis_top;
-    btVector3 axis_bottom;
-
-    btVector3 d_vector;   // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only.
-
-    // e_vector is constant, but depends on the joint type
     // prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
     // revolute: vector from parent's COM to the pivot point, in PARENT's frame.
-    btVector3 e_vector;
+    btVector3 m_eVector;
 
-    bool is_revolute;   // true = revolute, false = prismatic
+	btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
 
-    btQuaternion cached_rot_parent_to_this;   // rotates vectors in parent frame to vectors in local frame
+	enum eFeatherstoneJointType
-    btVector3 cached_r_vector;                // vector from COM of parent to COM of this link, in local frame.
+	{
+		eRevolute = 0,
+		ePrismatic = 1,
+		eSpherical = 2,
+#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+		ePlanar = 3,
+#endif
+		eInvalid
+	};
 
-    btVector3 applied_force;    // In WORLD frame
+	eFeatherstoneJointType m_jointType;
-    btVector3 applied_torque;   // In WORLD frame
+	int m_dofCount, m_posVarCount;				//redundant but handy
-    btScalar joint_torque;
+
+	// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
+    // for prismatic: m_axesTop[0] = zero;
+    //                m_axesBottom[0] = unit vector along the joint axis.
+    // for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
+    //               m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
+	//
+	// for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
+	//				  m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
+	//
+	// for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
+	//			   m_axesTop[1][2] = zero
+	//			   m_axesBottom[0] = zero
+	//			   m_axesBottom[1][2] = unit vectors along the translational axes on that plane		
+#ifndef TEST_SPATIAL_ALGEBRA_LAYER
+	btVector3 m_axesTop[6];
+	btVector3 m_axesBottom[6];	
+	void setAxisTop(int dof, const btVector3 &axis) { m_axesTop[dof] = axis; }
+	void setAxisBottom(int dof, const btVector3 &axis) { m_axesBottom[dof] = axis; }
+	void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axesTop[dof].setValue(x, y, z); }
+	void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axesBottom[dof].setValue(x, y, z); }
+	const btVector3 & getAxisTop(int dof) const  { return m_axesTop[dof]; }
+	const btVector3 & getAxisBottom(int dof) const { return m_axesBottom[dof]; }
+#else
+	btSpatialMotionVector m_axes[6];
+	void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
+	void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; }
+	void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_topVec.setValue(x, y, z); }
+	void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); }
+	const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
+	const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
+#endif
+
+	int m_dofOffset, m_cfgOffset;
+
+    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.
+
+    btVector3 m_appliedForce;    // In WORLD frame
+    btVector3 m_appliedTorque;   // In WORLD frame	
+
+	btScalar m_jointPos[7];
+	btScalar m_jointTorque[6];			//TODO
 
 	class btMultiBodyLinkCollider* m_collider;
 	int m_flags;
 
     // ctor: set some sensible defaults
 	btMultibodyLink()
-		: joint_pos(0),
+		: 	m_mass(1),
-			mass(1),
+			m_parent(-1),
-			parent(-1),
+			m_zeroRotParentToThis(1, 0, 0, 0),			
-			zero_rot_parent_to_this(1, 0, 0, 0),
+			m_cachedRotParentToThis(1, 0, 0, 0),			
-			is_revolute(false),
-			cached_rot_parent_to_this(1, 0, 0, 0),
-			joint_torque(0),
 			m_collider(0),
-			m_flags(0)
+			m_flags(0),
+			m_dofCount(0),
+			m_posVarCount(0),
+			m_jointType(btMultibodyLink::eInvalid)
 	{
-		inertia.setValue(1, 1, 1);
+		m_inertia.setValue(1, 1, 1);
-		axis_top.setValue(0, 0, 0);
+		setAxisTop(0, 0., 0., 0.);
-		axis_bottom.setValue(1, 0, 0);
+		setAxisBottom(0, 1., 0., 0.);
-		d_vector.setValue(0, 0, 0);
+		m_dVector.setValue(0, 0, 0);
-		e_vector.setValue(0, 0, 0);
+		m_eVector.setValue(0, 0, 0);
-		cached_r_vector.setValue(0, 0, 0);
+		m_cachedRVector.setValue(0, 0, 0);
-		applied_force.setValue( 0, 0, 0);
+		m_appliedForce.setValue( 0, 0, 0);
-		applied_torque.setValue(0, 0, 0);
+		m_appliedTorque.setValue(0, 0, 0);
+		//		
+		m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
+		m_jointPos[3] = 1.f;			//"quat.w"
+		m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
 	}
 
-    // routine to update cached_rot_parent_to_this and cached_r_vector
+    // routine to update m_cachedRotParentToThis and m_cachedRVector
     void updateCache()
 	{
-		if (is_revolute) 
+		//multidof
+		if (m_jointType == eRevolute) 
 		{
-			cached_rot_parent_to_this = btQuaternion(axis_top,-joint_pos) * zero_rot_parent_to_this;
+			m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis;
-			cached_r_vector = d_vector + quatRotate(cached_rot_parent_to_this,e_vector);
+			m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
 		} else 
 		{
-			// cached_rot_parent_to_this never changes, so no need to update
+			// m_cachedRotParentToThis never changes, so no need to update
-			cached_r_vector = e_vector + joint_pos * axis_bottom;
+			m_cachedRVector = m_eVector + m_jointPos[0] * getAxisBottom(0);
+		}
+	}
+
+	void updateCacheMultiDof(btScalar *pq = 0)
+	{
+		btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
+
+		switch(m_jointType)
+		{
+			case eRevolute:
+			{
+				m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
+				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
+
+				break;
+			}
+			case ePrismatic:
+			{
+				// m_cachedRotParentToThis never changes, so no need to update
+				m_cachedRVector = 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);
+
+				break;
+			}
+#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+			case ePlanar:
+			{
+				m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;				
+				m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * m_axesBottom[1] + pJointPos[2] * m_axesBottom[2]) + quatRotate(m_cachedRotParentToThis,m_eVector);				
+
+				break;
+			}
+#endif
 		}
 	}
 };

src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h

@@ -74,14 +74,14 @@ public:
 		if (m_link>=0)
 		{
 			const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
-			if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.parent == other->m_link)
+			if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link)
 				return false;
 		}
 		
 		if (other->m_link>=0)
 		{
 			const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
-			if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.parent == this->m_link)
+			if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link)
 				return false;
 		}
 		return true;

src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp

@@ -19,8 +19,14 @@ subject to the following restrictions:
 #include "btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
+#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
+	#define BTMBP2PCONSTRAINT_DIM 3
+#else
+	#define BTMBP2PCONSTRAINT_DIM 6
+#endif
+
 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
-	:btMultiBodyConstraint(body,0,link,-1,3,false),
+	:btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false),	
 	m_rigidBodyA(0),
 	m_rigidBodyB(bodyB),
 	m_pivotInA(pivotInA),
@@ -29,7 +35,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi
 }
 
 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
-	:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,3,false),
+	:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false),
 	m_rigidBodyA(0),
 	m_rigidBodyB(0),
 	m_pivotInA(pivotInA),
@@ -90,7 +96,7 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
 {
 
 //	int i=1;
-	for (int i=0;i<3;i++)
+	for (int i=0;i<BTMBP2PCONSTRAINT_DIM;i++)
 	{
 
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
@@ -98,9 +104,12 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
 		constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
 		constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
 		
-
 		btVector3 contactNormalOnB(0,0,0);
+#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
 		contactNormalOnB[i] = -1;
+#else
+		contactNormalOnB[i%3] = -1;
+#endif
 
 		btScalar penetration = 0;
 
@@ -127,17 +136,35 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
 				pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
 			
 		}
-		btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
-		btScalar relaxation = 1.f;
-		fillMultiBodyConstraintMixed(constraintRow, data,
-																 contactNormalOnB,
-																 pivotAworld, pivotBworld, 
-																 position,
-																 infoGlobal,
-																 relaxation,
-																 false);
-		constraintRow.m_lowerLimit = -m_maxAppliedImpulse;
-		constraintRow.m_upperLimit = m_maxAppliedImpulse;
 
+		btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0;
+
+#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST		
+		
+
+		fillMultiBodyConstraint(constraintRow, data, 0, 0,
+															contactNormalOnB, pivotAworld, pivotBworld,						//sucks but let it be this way "for the time being"
+															posError,
+															infoGlobal,
+															-m_maxAppliedImpulse, m_maxAppliedImpulse
+															);
+#else
+		const btVector3 dummy(0, 0, 0);
+
+		btAssert(m_bodyA->isMultiDof());
+
+		btScalar* jac1 = jacobianA(i);
+		const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
+		const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;
+
+		m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+
+		fillMultiBodyConstraint(constraintRow, data, jac1, 0,
+													dummy, dummy, dummy,						//sucks but let it be this way "for the time being"
+													posError,
+													infoGlobal,
+													-m_maxAppliedImpulse, m_maxAppliedImpulse
+													);
+#endif
 	}
 }

src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h

@@ -20,6 +20,8 @@ subject to the following restrictions:
 
 #include "btMultiBodyConstraint.h"
 
+//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
+
 class btMultiBodyPoint2Point : public btMultiBodyConstraint
 {
 protected:

src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h

@@ -28,16 +28,19 @@ ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
+	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_solverBodyIdB(-1), m_multiBodyA(0), m_multiBodyB(0), m_linkA(-1), m_linkB(-1)
+	{}
 
 	int				m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
+	int				m_jacAindex;
+	int				m_deltaVelBindex;
+	int				m_jacBindex;
+
 	btVector3		m_relpos1CrossNormal;
 	btVector3		m_contactNormal1;	
-	int				m_jacAindex;
-
-	int				m_deltaVelBindex;
 	btVector3		m_relpos2CrossNormal;
 	btVector3		m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
-	int				m_jacBindex;
+	
 
 	btVector3		m_angularComponentA;
 	btVector3		m_angularComponentB;