Initial Commit

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.h

@@ -0,0 +1,953 @@
+/*
+ * PURPOSE:
+ *   Class representing an articulated rigid body. Stores the body's
+ *   current state, allows forces and torques to be set, handles
+ *   timestepping and implements Featherstone's algorithm.
+ *   
+ * COPYRIGHT:
+ *   Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
+ *   Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
+ *   Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements
+
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ 
+ */
+
+#ifndef BT_MULTIBODY_H
+#define BT_MULTIBODY_H
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btQuaternion.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btMultiBodyData btMultiBodyDoubleData
+#define btMultiBodyDataName "btMultiBodyDoubleData"
+#define btMultiBodyLinkData btMultiBodyLinkDoubleData
+#define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData"
+#else
+#define btMultiBodyData btMultiBodyFloatData
+#define btMultiBodyDataName "btMultiBodyFloatData"
+#define btMultiBodyLinkData btMultiBodyLinkFloatData
+#define btMultiBodyLinkDataName "btMultiBodyLinkFloatData"
+#endif  //BT_USE_DOUBLE_PRECISION
+
+#include "btMultiBodyLink.h"
+class btMultiBodyLinkCollider;
+
+ATTRIBUTE_ALIGNED16(class)
+btMultiBody
+{
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	//
+	// initialization
+	//
+
+	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 fixedBase,            // whether the base is fixed (true) or can move (false)
+				bool canSleep, bool deprecatedMultiDof = true);
+
+	virtual ~btMultiBody();
+
+	//note: fixed link collision with parent is always disabled
+	void setupFixed(int i, //linkIndex
+					btScalar mass,
+					const btVector3 &inertia,
+					int parent,
+					const btQuaternion &rotParentToThis,
+					const btVector3 &parentComToThisPivotOffset,
+					const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision = true);
+
+	void setupPrismatic(int i,
+						btScalar mass,
+						const btVector3 &inertia,
+						int parent,
+						const btQuaternion &rotParentToThis,
+						const btVector3 &jointAxis,
+						const btVector3 &parentComToThisPivotOffset,
+						const btVector3 &thisPivotToThisComOffset,
+						bool disableParentCollision);
+
+	void setupRevolute(int i,  // 0 to num_links-1
+					   btScalar mass,
+					   const btVector3 &inertia,
+					   int parentIndex,
+					   const btQuaternion &rotParentToThis,          // rotate points in parent frame to this frame, when q = 0
+					   const btVector3 &jointAxis,                   // in my frame
+					   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);
+
+	void setupSpherical(int i,  // linkIndex, 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);
+
+	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);
+
+	const btMultibodyLink &getLink(int index) const
+	{
+		return m_links[index];
+	}
+
+	btMultibodyLink &getLink(int index)
+	{
+		return m_links[index];
+	}
+
+	void setBaseCollider(btMultiBodyLinkCollider * collider)  //collider can be NULL to disable collision for the base
+	{
+		m_baseCollider = collider;
+	}
+	const btMultiBodyLinkCollider *getBaseCollider() const
+	{
+		return m_baseCollider;
+	}
+	btMultiBodyLinkCollider *getBaseCollider()
+	{
+		return m_baseCollider;
+	}
+
+	const btMultiBodyLinkCollider *getLinkCollider(int index) const
+	{
+		if (index >= 0 && index < getNumLinks())
+		{
+			return getLink(index).m_collider;
+		}
+		return 0;
+	}
+
+	btMultiBodyLinkCollider *getLinkCollider(int index)
+	{
+		if (index >= 0 && index < getNumLinks())
+		{
+			return getLink(index).m_collider;
+		}
+		return 0;
+	}
+
+	//
+	// get parent
+	// input: link num from 0 to num_links-1
+	// output: link num from 0 to num_links-1, OR -1 to mean the base.
+	//
+	int getParent(int link_num) const;
+
+	//
+	// get number of m_links, masses, moments of inertia
+	//
+
+	int getNumLinks() const { return m_links.size(); }
+	int getNumDofs() const { return m_dofCount; }
+	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;
+
+	//
+	// change mass (incomplete: can only change base mass and inertia at present)
+	//
+
+	void setBaseMass(btScalar mass) { m_baseMass = mass; }
+	void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; }
+
+	//
+	// get/set pos/vel/rot/omega for the base link
+	//
+
+	const btVector3 &getBasePos() const 
+	{ 
+		return m_basePos; 
+	}  // in world frame
+	const btVector3 getBaseVel() const
+	{
+		return btVector3(m_realBuf[3], m_realBuf[4], m_realBuf[5]);
+	}  // in world frame
+	const btQuaternion &getWorldToBaseRot() const
+	{
+		return m_baseQuat;
+	}
+    
+    const btVector3 &getInterpolateBasePos() const
+    {
+        return m_basePos_interpolate;
+    }  // in world frame
+    const btQuaternion &getInterpolateWorldToBaseRot() const
+    {
+        return m_baseQuat_interpolate;
+    }
+    
+    // rotates world vectors into base frame
+	btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); }  // in world frame
+
+	void setBasePos(const btVector3 &pos)
+	{
+		m_basePos = pos;
+		if(!isBaseKinematic())
+			m_basePos_interpolate = pos;
+	}
+
+	void setInterpolateBasePos(const btVector3 &pos)
+	{
+		m_basePos_interpolate = pos;
+	}
+
+	void setBaseWorldTransform(const btTransform &tr)
+	{
+		setBasePos(tr.getOrigin());
+		setWorldToBaseRot(tr.getRotation().inverse());
+	}
+
+	btTransform getBaseWorldTransform() const
+	{
+		btTransform tr;
+		tr.setOrigin(getBasePos());
+		tr.setRotation(getWorldToBaseRot().inverse());
+		return tr;
+	}
+
+	void setInterpolateBaseWorldTransform(const btTransform &tr)
+	{
+		setInterpolateBasePos(tr.getOrigin());
+		setInterpolateWorldToBaseRot(tr.getRotation().inverse());
+	}
+
+	btTransform getInterpolateBaseWorldTransform() const
+	{
+		btTransform tr;
+		tr.setOrigin(getInterpolateBasePos());
+		tr.setRotation(getInterpolateWorldToBaseRot().inverse());
+		return tr;
+	}
+
+	void setBaseVel(const btVector3 &vel)
+	{
+		m_realBuf[3] = vel[0];
+		m_realBuf[4] = vel[1];
+		m_realBuf[5] = vel[2];
+	}
+
+	void setWorldToBaseRot(const btQuaternion &rot)
+	{
+		m_baseQuat = rot;  //m_baseQuat asumed to ba alias!?
+		if(!isBaseKinematic())
+			m_baseQuat_interpolate = rot;
+	}
+
+	void setInterpolateWorldToBaseRot(const btQuaternion &rot)
+	{
+		m_baseQuat_interpolate = rot;
+	}
+
+	void setBaseOmega(const btVector3 &omega)
+	{
+		m_realBuf[0] = omega[0];
+		m_realBuf[1] = omega[1];
+		m_realBuf[2] = omega[2];
+	}
+
+	void saveKinematicState(btScalar timeStep);
+
+	//
+	// 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);
+
+	const btScalar *getJointVelMultiDof(int i) const;
+	const btScalar *getJointPosMultiDof(int i) const;
+
+	void setJointPos(int i, btScalar q);
+	void setJointVel(int i, btScalar qdot);
+	void setJointPosMultiDof(int i, const double *q);
+	void setJointVelMultiDof(int i, const double *qdot);
+	void setJointPosMultiDof(int i, const float *q);
+	void setJointVelMultiDof(int i, const float *qdot);
+
+	//
+	// direct access to velocities as a vector of 6 + num_links elements.
+	// (omega first, then v, then joint velocities.)
+	//
+	const btScalar *getVelocityVector() const
+	{
+		return &m_realBuf[0];
+	}
+    
+    const btScalar *getDeltaVelocityVector() const
+    {
+        return &m_deltaV[0];
+    }
+    
+    const btScalar *getSplitVelocityVector() const
+    {
+        return &m_splitV[0];
+    }
+	/*    btScalar * getVelocityVector() 
+	{ 
+		return &real_buf[0]; 
+	}
+  */
+
+	//
+	// get the frames of reference (positions and orientations) of the child m_links
+	// (i = 0 to num_links-1)
+	//
+
+	const btVector3 &getRVector(int i) const;              // vector from COM(parent(i)) to COM(i), in frame i's coords
+	const btQuaternion &getParentToLocalRot(int i) const;  // rotates vectors in frame parent(i) to vectors in frame i.
+    const btVector3 &getInterpolateRVector(int i) const;              // vector from COM(parent(i)) to COM(i), in frame i's coords
+    const btQuaternion &getInterpolateParentToLocalRot(int i) const;  // rotates vectors in frame parent(i) to vectors in frame i.
+
+	//
+	// transform vectors in local frame of link i to world frame (or vice versa)
+	//
+	btVector3 localPosToWorld(int i, const btVector3 &local_pos) const;
+	btVector3 localDirToWorld(int i, const btVector3 &local_dir) const;
+	btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const;
+	btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const;
+
+	//
+	// transform a frame in local coordinate to a frame in world coordinate
+	//
+	btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const;
+
+
+	//
+	// set external forces and torques. Note all external forces/torques are given in the WORLD frame.
+	//
+
+	void clearForcesAndTorques();
+	void clearConstraintForces();
+
+	void clearVelocities();
+
+	void addBaseForce(const btVector3 &f)
+	{
+		m_baseForce += f;
+	}
+	void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
+	void addLinkForce(int i, const btVector3 &f);
+	void addLinkTorque(int i, const btVector3 &t);
+
+	void addBaseConstraintForce(const btVector3 &f)
+	{
+		m_baseConstraintForce += f;
+	}
+	void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; }
+	void addLinkConstraintForce(int i, const btVector3 &f);
+	void addLinkConstraintTorque(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 m_baseForce; }
+	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);
+
+	//
+	// dynamics routines.
+	//
+
+	// timestep the velocities (given the external forces/torques set using addBaseForce etc).
+	// also sets up caches for calcAccelerationDeltas.
+	//
+	// Note: the caller must provide three vectors which are used as
+	// temporary scratch space. The idea here is to reduce dynamic
+	// memory allocation: the same scratch vectors can be re-used
+	// again and again for different Multibodies, instead of each
+	// btMultiBody allocating (and then deallocating) their own
+	// individual scratch buffers. This gives a considerable speed
+	// improvement, at least on Windows (where dynamic memory
+	// allocation appears to be fairly slow).
+	//
+
+	void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
+															  btAlignedObjectArray<btScalar> & scratch_r,
+															  btAlignedObjectArray<btVector3> & scratch_v,
+															  btAlignedObjectArray<btMatrix3x3> & scratch_m,
+															  bool isConstraintPass,
+                                                              bool jointFeedbackInWorldSpace,
+                                                              bool jointFeedbackInJointFrame
+                                                              );
+
+	///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead
+	//void stepVelocitiesMultiDof(btScalar dt,
+	//							btAlignedObjectArray<btScalar> & scratch_r,
+	//							btAlignedObjectArray<btVector3> & scratch_v,
+	//							btAlignedObjectArray<btMatrix3x3> & scratch_m,
+	//							bool isConstraintPass = false)
+	//{
+	//	computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false);
+	//}
+
+	// calcAccelerationDeltasMultiDof
+	// input: force vector (in same format as jacobian, i.e.:
+	//                      3 torque values, 3 force values, num_links joint torque values)
+	// output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
+	// (existing contents of output array are replaced)
+	// calcAccelerationDeltasMultiDof must have been called first.
+	void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
+										btAlignedObjectArray<btScalar> &scratch_r,
+										btAlignedObjectArray<btVector3> &scratch_v) const;
+
+	void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
+	{
+		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+		{
+			m_deltaV[dof] += delta_vee[dof] * multiplier;
+		}
+	}
+    void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
+    {
+        for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+        {
+            m_splitV[dof] += delta_vee[dof] * multiplier;
+        }
+    }
+    void addSplitV()
+    {
+        applyDeltaVeeMultiDof(&m_splitV[0], 1);
+    }
+    void substractSplitV()
+    {
+        applyDeltaVeeMultiDof(&m_splitV[0], -1);
+        
+        for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+        {
+            m_splitV[dof] = 0.f;
+        }
+    }
+	void processDeltaVeeMultiDof2()
+	{
+		applyDeltaVeeMultiDof(&m_deltaV[0], 1);
+
+		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+		{
+			m_deltaV[dof] = 0.f;
+		}
+	}
+
+	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;
+			btClamp(m_realBuf[dof], -m_maxCoordinateVelocity, m_maxCoordinateVelocity);
+		}
+	}
+
+	// timestep the positions (given current velocities).
+	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
+    
+    // predict the positions
+    void predictPositionsMultiDof(btScalar dt);
+
+	//
+	// contacts
+	//
+
+	// This routine fills out a contact constraint jacobian for this body.
+	// the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
+	// 'normal' & 'contact_point' are both given in world coordinates.
+
+	void 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 { fillConstraintJacobianMultiDof(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 fillConstraintJacobianMultiDof(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)
+	{
+		if (m_canWakeup)
+		{
+			m_canSleep = canSleep;
+		}
+	}
+
+	bool getCanSleep() const
+	{
+		return m_canSleep;
+	}
+
+	bool getCanWakeup() const
+	{
+		return m_canWakeup;
+	}
+	
+	void setCanWakeup(bool canWakeup) 
+	{
+		m_canWakeup = canWakeup;
+	}
+	bool isAwake() const 
+	{ 
+		return m_awake; 
+	}
+	void wakeUp();
+	void goToSleep();
+	void checkMotionAndSleepIfRequired(btScalar timestep);
+
+	bool hasFixedBase() const;
+
+	bool isBaseKinematic() const;
+
+	bool isBaseStaticOrKinematic() const;
+
+	// set the dynamic type in the base's collision flags.
+	void setBaseDynamicType(int dynamicType);
+
+	void setFixedBase(bool fixedBase)
+	{
+		m_fixedBase = fixedBase;
+		if(m_fixedBase)
+			setBaseDynamicType(btCollisionObject::CF_STATIC_OBJECT);
+		else
+			setBaseDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT);
+	}
+
+	int getCompanionId() const
+	{
+		return m_companionId;
+	}
+	void setCompanionId(int id)
+	{
+		//printf("for %p setCompanionId(%d)\n",this, id);
+		m_companionId = id;
+	}
+
+	void setNumLinks(int numLinks)  //careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
+	{
+		m_links.resize(numLinks);
+	}
+
+	btScalar getLinearDamping() const
+	{
+		return m_linearDamping;
+	}
+	void setLinearDamping(btScalar damp)
+	{
+		m_linearDamping = damp;
+	}
+	btScalar getAngularDamping() const
+	{
+		return m_angularDamping;
+	}
+	void setAngularDamping(btScalar damp)
+	{
+		m_angularDamping = damp;
+	}
+
+	bool getUseGyroTerm() const
+	{
+		return m_useGyroTerm;
+	}
+	void setUseGyroTerm(bool useGyro)
+	{
+		m_useGyroTerm = useGyro;
+	}
+	btScalar getMaxCoordinateVelocity() const
+	{
+		return m_maxCoordinateVelocity;
+	}
+	void setMaxCoordinateVelocity(btScalar maxVel)
+	{
+		m_maxCoordinateVelocity = maxVel;
+	}
+
+	btScalar getMaxAppliedImpulse() const
+	{
+		return m_maxAppliedImpulse;
+	}
+	void setMaxAppliedImpulse(btScalar maxImp)
+	{
+		m_maxAppliedImpulse = maxImp;
+	}
+	void setHasSelfCollision(bool hasSelfCollision)
+	{
+		m_hasSelfCollision = hasSelfCollision;
+	}
+	bool hasSelfCollision() const
+	{
+		return m_hasSelfCollision;
+	}
+
+	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 isPosUpdated() const
+	{
+		return __posUpdated;
+	}
+	void setPosUpdated(bool updated)
+	{
+		__posUpdated = updated;
+	}
+
+	//internalNeedsJointFeedback is for internal use only
+	bool internalNeedsJointFeedback() const
+	{
+		return m_internalNeedsJointFeedback;
+	}
+	void forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local, btAlignedObjectArray<btVector3> & local_origin);
+
+	void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const;
+
+	void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
+    void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
+
+	virtual int calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual const char *serialize(void *dataBuffer, class btSerializer *serializer) const;
+
+	const char *getBaseName() const
+	{
+		return m_baseName;
+	}
+	///memory of setBaseName needs to be manager by user
+	void setBaseName(const char *name)
+	{
+		m_baseName = name;
+	}
+
+	///users can point to their objects, userPointer is not used by Bullet
+	void *getUserPointer() const
+	{
+		return m_userObjectPointer;
+	}
+
+	int getUserIndex() const
+	{
+		return m_userIndex;
+	}
+
+	int getUserIndex2() const
+	{
+		return m_userIndex2;
+	}
+	///users can point to their objects, userPointer is not used by Bullet
+	void setUserPointer(void *userPointer)
+	{
+		m_userObjectPointer = userPointer;
+	}
+
+	///users can point to their objects, userPointer is not used by Bullet
+	void setUserIndex(int index)
+	{
+		m_userIndex = index;
+	}
+
+	void setUserIndex2(int index)
+	{
+		m_userIndex2 = index;
+	}
+
+	static void spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
+		const btVector3 &displacement,     // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
+		const btVector3 &top_in,       // top part of input vector
+		const btVector3 &bottom_in,    // bottom part of input vector
+		btVector3 &top_out,         // top part of output vector
+		btVector3 &bottom_out);      // bottom part of output vector
+
+	void setLinkDynamicType(const int i, int type);
+
+	bool isLinkStaticOrKinematic(const int i) const;
+
+	bool isLinkKinematic(const int i) const;
+
+	bool isLinkAndAllAncestorsStaticOrKinematic(const int i) const;
+
+	bool isLinkAndAllAncestorsKinematic(const int i) const;
+
+	void setSleepThreshold(btScalar sleepThreshold)
+	{
+		m_sleepEpsilon = sleepThreshold;
+	}
+
+	void setSleepTimeout(btScalar sleepTimeout)
+	{
+		this->m_sleepTimeout = sleepTimeout;
+	}
+
+
+private:
+	btMultiBody(const btMultiBody &);     // not implemented
+	void operator=(const btMultiBody &);  // not implemented
+
+	void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const;
+	void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
+
+	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(const btScalar *pA, const btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
+
+private:
+	btMultiBodyLinkCollider *m_baseCollider;  //can be NULL
+	const char *m_baseName;                   //memory needs to be manager by user!
+
+	btVector3 m_basePos;      // position of COM of base (world frame)
+    btVector3 m_basePos_interpolate;      // position of interpolated COM of base (world frame)
+	btQuaternion m_baseQuat;  // rotates world points into base frame
+    btQuaternion m_baseQuat_interpolate;  
+
+	btScalar m_baseMass;      // mass of the base
+	btVector3 m_baseInertia;  // inertia of the base (in local frame; diagonal)
+
+	btVector3 m_baseForce;   // external force applied to base. World frame.
+	btVector3 m_baseTorque;  // external torque applied to base. World frame.
+
+	btVector3 m_baseConstraintForce;   // external force applied to base. World frame.
+	btVector3 m_baseConstraintTorque;  // external torque applied to base. World frame.
+
+	btAlignedObjectArray<btMultibodyLink> m_links;  // array of m_links, excluding the base. index from 0 to num_links-1.
+
+	//
+	// realBuf:
+	//  offset         size            array
+	//   0              6 + num_links   v (base_omega; base_vel; joint_vels)					MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized]
+	//   6+num_links    num_links       D
+	//
+	// vectorBuf:
+	//  offset         size         array
+	//   0              num_links    h_top
+	//   num_links      num_links    h_bottom
+	//
+	// matrixBuf:
+	//  offset         size         array
+	//   0              num_links+1  rot_from_parent
+	//
+    btAlignedObjectArray<btScalar> m_splitV;
+	btAlignedObjectArray<btScalar> m_deltaV;
+	btAlignedObjectArray<btScalar> m_realBuf;
+	btAlignedObjectArray<btVector3> m_vectorBuf;
+	btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
+
+	btMatrix3x3 m_cachedInertiaTopLeft;
+	btMatrix3x3 m_cachedInertiaTopRight;
+	btMatrix3x3 m_cachedInertiaLowerLeft;
+	btMatrix3x3 m_cachedInertiaLowerRight;
+	bool m_cachedInertiaValid;
+
+	bool m_fixedBase;
+
+	// Sleep parameters.
+	bool m_awake;
+	bool m_canSleep;
+	bool m_canWakeup;
+	btScalar m_sleepTimer;
+	btScalar m_sleepEpsilon;
+	btScalar m_sleepTimeout;
+
+	void *m_userObjectPointer;
+	int m_userIndex2;
+	int m_userIndex;
+
+	int m_companionId;
+	btScalar m_linearDamping;
+	btScalar m_angularDamping;
+	bool m_useGyroTerm;
+	btScalar m_maxAppliedImpulse;
+	btScalar m_maxCoordinateVelocity;
+	bool m_hasSelfCollision;
+
+	bool __posUpdated;
+	int m_dofCount, m_posVarCnt;
+
+	bool m_useRK4, m_useGlobalVelocities;
+	//for global velocities, see 8.3.2B Proposed resolution in Jakub Stepien PhD Thesis
+	//https://drive.google.com/file/d/0Bz3vEa19XOYGNWdZWGpMdUdqVmZ5ZVBOaEh4ZnpNaUxxZFNV/view?usp=sharing
+
+	///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
+	bool m_internalNeedsJointFeedback;
+
+  //If enabled, calculate the velocity based on kinematic transform changes. Currently only implemented for the base.
+	bool m_kinematic_calculate_velocity;
+};
+
+struct btMultiBodyLinkDoubleData
+{
+	btQuaternionDoubleData m_zeroRotParentToThis;
+	btVector3DoubleData m_parentComToThisPivotOffset;
+	btVector3DoubleData m_thisPivotToThisComOffset;
+	btVector3DoubleData m_jointAxisTop[6];
+	btVector3DoubleData m_jointAxisBottom[6];
+
+	btVector3DoubleData m_linkInertia;  // inertia of the base (in local frame; diagonal)
+	btVector3DoubleData m_absFrameTotVelocityTop;
+	btVector3DoubleData m_absFrameTotVelocityBottom;
+	btVector3DoubleData m_absFrameLocVelocityTop;
+	btVector3DoubleData m_absFrameLocVelocityBottom;
+
+	double m_linkMass;
+	int m_parentIndex;
+	int m_jointType;
+
+	int m_dofCount;
+	int m_posVarCount;
+	double m_jointPos[7];
+	double m_jointVel[6];
+	double m_jointTorque[6];
+
+	double m_jointDamping;
+	double m_jointFriction;
+	double m_jointLowerLimit;
+	double m_jointUpperLimit;
+	double m_jointMaxForce;
+	double m_jointMaxVelocity;
+
+	char *m_linkName;
+	char *m_jointName;
+	btCollisionObjectDoubleData *m_linkCollider;
+	char *m_paddingPtr;
+};
+
+struct btMultiBodyLinkFloatData
+{
+	btQuaternionFloatData m_zeroRotParentToThis;
+	btVector3FloatData m_parentComToThisPivotOffset;
+	btVector3FloatData m_thisPivotToThisComOffset;
+	btVector3FloatData m_jointAxisTop[6];
+	btVector3FloatData m_jointAxisBottom[6];
+	btVector3FloatData m_linkInertia;  // inertia of the base (in local frame; diagonal)
+	btVector3FloatData m_absFrameTotVelocityTop;
+	btVector3FloatData m_absFrameTotVelocityBottom;
+	btVector3FloatData m_absFrameLocVelocityTop;
+	btVector3FloatData m_absFrameLocVelocityBottom;
+
+	int m_dofCount;
+	float m_linkMass;
+	int m_parentIndex;
+	int m_jointType;
+
+	float m_jointPos[7];
+	float m_jointVel[6];
+	float m_jointTorque[6];
+	int m_posVarCount;
+	float m_jointDamping;
+	float m_jointFriction;
+	float m_jointLowerLimit;
+	float m_jointUpperLimit;
+	float m_jointMaxForce;
+	float m_jointMaxVelocity;
+
+	char *m_linkName;
+	char *m_jointName;
+	btCollisionObjectFloatData *m_linkCollider;
+	char *m_paddingPtr;
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btMultiBodyDoubleData
+{
+	btVector3DoubleData m_baseWorldPosition;
+	btQuaternionDoubleData m_baseWorldOrientation;
+	btVector3DoubleData m_baseLinearVelocity;
+	btVector3DoubleData m_baseAngularVelocity;
+	btVector3DoubleData m_baseInertia;  // inertia of the base (in local frame; diagonal)
+	double m_baseMass;
+	int m_numLinks;
+	char m_padding[4];
+
+	char *m_baseName;
+	btMultiBodyLinkDoubleData *m_links;
+	btCollisionObjectDoubleData *m_baseCollider;
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btMultiBodyFloatData
+{
+	btVector3FloatData m_baseWorldPosition;
+	btQuaternionFloatData m_baseWorldOrientation;
+	btVector3FloatData m_baseLinearVelocity;
+	btVector3FloatData m_baseAngularVelocity;
+
+	btVector3FloatData m_baseInertia;  // inertia of the base (in local frame; diagonal)
+	float m_baseMass;
+	int m_numLinks;
+
+	char *m_baseName;
+	btMultiBodyLinkFloatData *m_links;
+	btCollisionObjectFloatData *m_baseCollider;
+};
+
+#endif

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -0,0 +1,389 @@
+#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, int type)
+	: m_bodyA(bodyA),
+	  m_bodyB(bodyB),
+	  m_linkA(linkA),
+	  m_linkB(linkB),
+	  m_type(type),
+	  m_numRows(numRows),
+	  m_jacSizeA(0),
+	  m_jacSizeBoth(0),
+	  m_isUnilateral(isUnilateral),
+	  m_numDofsFinalized(-1),
+	  m_maxAppliedImpulse(100)
+{
+}
+
+void btMultiBodyConstraint::updateJacobianSizes()
+{
+	if (m_bodyA)
+	{
+		m_jacSizeA = (6 + m_bodyA->getNumDofs());
+	}
+
+	if (m_bodyB)
+	{
+		m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
+	}
+	else
+		m_jacSizeBoth = m_jacSizeA;
+}
+
+void btMultiBodyConstraint::allocateJacobiansMultiDof()
+{
+	updateJacobianSizes();
+
+	m_posOffset = ((1 + m_jacSizeBoth) * m_numRows);
+	m_data.resize((2 + m_jacSizeBoth) * m_numRows);
+}
+
+btMultiBodyConstraint::~btMultiBodyConstraint()
+{
+}
+
+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,
+														btMultiBodyJacobianData& data,
+														btScalar* jacOrgA, btScalar* jacOrgB,
+														const btVector3& constraintNormalAng,
+														const btVector3& constraintNormalLin,
+														const btVector3& posAworld, const btVector3& posBworld,
+														btScalar posError,
+														const btContactSolverInfo& infoGlobal,
+														btScalar lowerLimit, btScalar upperLimit,
+														bool angConstraint,
+														btScalar relaxation,
+														bool isFriction, btScalar desiredVelocity, btScalar cfmSlip,
+														btScalar damping)
+{
+	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;
+
+	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 = posAworld - bodyA->getWorldTransform().getOrigin();
+	if (bodyB)
+		rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
+
+	if (multiBodyA)
+	{
+		if (solverConstraint.m_linkA < 0)
+		{
+			rel_pos1 = posAworld - multiBodyA->getBasePos();
+		}
+		else
+		{
+			rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
+		}
+
+		const int ndofA = multiBodyA->getNumDofs() + 6;
+
+		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
+
+		if (solverConstraint.m_deltaVelAindex < 0)
+		{
+			solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
+			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
+			data.m_deltaVelocities.resize(data.m_deltaVelocities.size() + ndofA);
+		}
+		else
+		{
+			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);
+		//copy/determine
+		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];
+			//multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+			multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, 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..
+		multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex], delta, data.scratch_r, data.scratch_v);
+
+		btVector3 torqueAxis0;
+		if (angConstraint)
+		{
+			torqueAxis0 = constraintNormalAng;
+		}
+		else
+		{
+			torqueAxis0 = rel_pos1.cross(constraintNormalLin);
+		}
+		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+		solverConstraint.m_contactNormal1 = constraintNormalLin;
+	}
+	else  //if(rb0)
+	{
+		btVector3 torqueAxis0;
+		if (angConstraint)
+		{
+			torqueAxis0 = constraintNormalAng;
+		}
+		else
+		{
+			torqueAxis0 = rel_pos1.cross(constraintNormalLin);
+		}
+		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
+		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+		solverConstraint.m_contactNormal1 = constraintNormalLin;
+	}
+
+	if (multiBodyB)
+	{
+		if (solverConstraint.m_linkB < 0)
+		{
+			rel_pos2 = posBworld - multiBodyB->getBasePos();
+		}
+		else
+		{
+			rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
+		}
+
+		const int ndofB = multiBodyB->getNumDofs() + 6;
+
+		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
+		if (solverConstraint.m_deltaVelBindex < 0)
+		{
+			solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
+			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
+			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
+		{
+			//multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+			multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &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..
+		multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex], delta, data.scratch_r, data.scratch_v);
+
+		btVector3 torqueAxis1;
+		if (angConstraint)
+		{
+			torqueAxis1 = constraintNormalAng;
+		}
+		else
+		{
+			torqueAxis1 = rel_pos2.cross(constraintNormalLin);
+		}
+		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+		solverConstraint.m_contactNormal2 = -constraintNormalLin;
+	}
+	else  //if(rb1)
+	{
+		btVector3 torqueAxis1;
+		if (angConstraint)
+		{
+			torqueAxis1 = constraintNormalAng;
+		}
+		else
+		{
+			torqueAxis1 = rel_pos2.cross(constraintNormalLin);
+		}
+		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
+		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+		solverConstraint.m_contactNormal2 = -constraintNormalLin;
+	}
+	{
+		btVector3 vec;
+		btScalar denom0 = 0.f;
+		btScalar denom1 = 0.f;
+		btScalar* jacB = 0;
+		btScalar* jacA = 0;
+		btScalar* deltaVelA = 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->getNumDofs() + 6;
+			jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+			deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+			for (int i = 0; i < ndofA; ++i)
+			{
+				btScalar j = jacA[i];
+				btScalar l = deltaVelA[i];
+				denom0 += j * l;
+			}
+		}
+		else if (rb0)
+		{
+			vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
+			if (angConstraint)
+			{
+				denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA);
+			}
+			else
+			{
+				denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
+			}
+		}
+		//
+		if (multiBodyB)
+		{
+			const int ndofB = multiBodyB->getNumDofs() + 6;
+			jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+			deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+			for (int i = 0; i < ndofB; ++i)
+			{
+				btScalar j = jacB[i];
+				btScalar l = deltaVelB[i];
+				denom1 += j * l;
+			}
+		}
+		else if (rb1)
+		{
+			vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
+			if (angConstraint)
+			{
+				denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB);
+			}
+			else
+			{
+				denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
+			}
+		}
+
+		//
+		btScalar d = denom0 + denom1;
+		if (d > SIMD_EPSILON)
+		{
+			solverConstraint.m_jacDiagABInv = relaxation / (d);
+		}
+		else
+		{
+			//disable the constraint row to handle singularity/redundant constraint
+			solverConstraint.m_jacDiagABInv = 0.f;
+		}
+	}
+
+	//compute rhs and remaining solverConstraint fields
+	btScalar penetration = isFriction ? 0 : posError;
+
+	btScalar rel_vel = 0.f;
+	int ndofA = 0;
+	int ndofB = 0;
+	{
+		btVector3 vel1, vel2;
+		if (multiBodyA)
+		{
+			ndofA = multiBodyA->getNumDofs() + 6;
+			btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+			for (int i = 0; i < ndofA; ++i)
+				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
+		}
+		else if (rb0)
+		{
+			rel_vel += rb0->getLinearVelocity().dot(solverConstraint.m_contactNormal1);
+			rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal);
+		}
+		if (multiBodyB)
+		{
+			ndofB = multiBodyB->getNumDofs() + 6;
+			btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+			for (int i = 0; i < ndofB; ++i)
+				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
+		}
+		else if (rb1)
+		{
+			rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2);
+			rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal);
+		}
+
+		solverConstraint.m_friction = 0.f;  //cp.m_combinedFriction;
+	}
+
+	solverConstraint.m_appliedImpulse = 0.f;
+	solverConstraint.m_appliedPushImpulse = 0.f;
+
+	{
+		btScalar positionalError = 0.f;
+		btScalar velocityError = (desiredVelocity - rel_vel) * damping;
+
+		btScalar erp = infoGlobal.m_erp2;
+
+		//split impulse is not implemented yet for btMultiBody*
+		//if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		{
+			erp = infoGlobal.m_erp;
+		}
+
+		positionalError = -penetration * erp / infoGlobal.m_timeStep;
+
+		btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
+		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
+
+		//split impulse is not implemented yet for btMultiBody*
+
+		//  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		{
+			//combine position and velocity into rhs
+			solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
+			solverConstraint.m_rhsPenetration = 0.f;
+		}
+		/*else
+        {
+            //split position and velocity into rhs and m_rhsPenetration
+            solverConstraint.m_rhs = velocityImpulse;
+            solverConstraint.m_rhsPenetration = penetrationImpulse;
+        }
+        */
+
+		solverConstraint.m_cfm = 0.f;
+		solverConstraint.m_lowerLimit = lowerLimit;
+		solverConstraint.m_upperLimit = upperLimit;
+	}
+
+	return rel_vel;
+}

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -0,0 +1,215 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_CONSTRAINT_H
+#define BT_MULTIBODY_CONSTRAINT_H
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btMultiBody.h"
+
+
+//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
+enum btTypedMultiBodyConstraintType
+{
+	MULTIBODY_CONSTRAINT_LIMIT=3,
+	MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR,
+	MULTIBODY_CONSTRAINT_GEAR,
+	MULTIBODY_CONSTRAINT_POINT_TO_POINT,
+	MULTIBODY_CONSTRAINT_SLIDER,
+	MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR,
+	MULTIBODY_CONSTRAINT_FIXED,
+	
+	MAX_MULTIBODY_CONSTRAINT_TYPE,
+};
+
+class btMultiBody;
+struct btSolverInfo;
+
+#include "btMultiBodySolverConstraint.h"
+
+struct btMultiBodyJacobianData
+{
+	btAlignedObjectArray<btScalar> m_jacobians;
+	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;             //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;
+	btAlignedObjectArray<btSolverBody>* m_solverBodyPool;
+	int m_fixedBodyId;
+};
+
+ATTRIBUTE_ALIGNED16(class)
+btMultiBodyConstraint
+{
+protected:
+	btMultiBody* m_bodyA;
+	btMultiBody* m_bodyB;
+	int m_linkA;
+	int m_linkB;
+
+	int m_type; //btTypedMultiBodyConstraintType
+
+	int m_numRows;
+	int m_jacSizeA;
+	int m_jacSizeBoth;
+	int m_posOffset;
+
+	bool m_isUnilateral;
+	int m_numDofsFinalized;
+	btScalar m_maxAppliedImpulse;
+
+	// warning: the data block lay out is not consistent for all constraints
+	// data block laid out as follows:
+	// cached impulses. (one per row.)
+	// jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
+	// positions. (one per row.)
+	btAlignedObjectArray<btScalar> m_data;
+
+	void applyDeltaVee(btMultiBodyJacobianData & data, btScalar * delta_vee, btScalar impulse, int velocityIndex, int ndof);
+
+	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint & solverConstraint,
+									 btMultiBodyJacobianData & data,
+									 btScalar * jacOrgA, btScalar * jacOrgB,
+									 const btVector3& constraintNormalAng,
+
+									 const btVector3& constraintNormalLin,
+									 const btVector3& posAworld, const btVector3& posBworld,
+									 btScalar posError,
+									 const btContactSolverInfo& infoGlobal,
+									 btScalar lowerLimit, btScalar upperLimit,
+									 bool angConstraint = false,
+
+									 btScalar relaxation = 1.f,
+									 bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 0, btScalar damping = 1.0);
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type);
+	virtual ~btMultiBodyConstraint();
+
+	void updateJacobianSizes();
+	void allocateJacobiansMultiDof();
+
+	int getConstraintType() const
+	{
+		return m_type;
+	}
+	//many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later.
+	virtual void setFrameInB(const btMatrix3x3& frameInB) {}
+	virtual void setPivotInB(const btVector3& pivotInB) {}
+
+	virtual void finalizeMultiDof() = 0;
+
+	virtual int getIslandIdA() const = 0;
+	virtual int getIslandIdB() const = 0;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows,
+									  btMultiBodyJacobianData & data,
+									  const btContactSolverInfo& infoGlobal) = 0;
+
+	int getNumRows() const
+	{
+		return m_numRows;
+	}
+
+	btMultiBody* getMultiBodyA()
+	{
+		return m_bodyA;
+	}
+	btMultiBody* getMultiBodyB()
+	{
+		return m_bodyB;
+	}
+
+	int getLinkA() const
+	{
+		return m_linkA;
+	}
+	int getLinkB() const
+	{
+		return m_linkB;
+	}
+	void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
+	{
+		btAssert(dof >= 0);
+		btAssert(dof < getNumRows());
+		m_data[dof] = appliedImpulse;
+	}
+
+	btScalar getAppliedImpulse(int dof)
+	{
+		btAssert(dof >= 0);
+		btAssert(dof < getNumRows());
+		return m_data[dof];
+	}
+	// current constraint position
+	// constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
+	// NOTE: ignored position for friction rows.
+	btScalar getPosition(int row) const
+	{
+		return m_data[m_posOffset + row];
+	}
+
+	void setPosition(int row, btScalar pos)
+	{
+		m_data[m_posOffset + row] = pos;
+	}
+
+	bool isUnilateral() const
+	{
+		return m_isUnilateral;
+	}
+
+	// jacobian blocks.
+	// each of size 6 + num_links. (jacobian2 is null if no body2.)
+	// format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
+	btScalar* jacobianA(int row)
+	{
+		return &m_data[m_numRows + row * m_jacSizeBoth];
+	}
+	const btScalar* jacobianA(int row) const
+	{
+		return &m_data[m_numRows + (row * m_jacSizeBoth)];
+	}
+	btScalar* jacobianB(int row)
+	{
+		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
+	}
+	const btScalar* jacobianB(int row) const
+	{
+		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
+	}
+
+	btScalar getMaxAppliedImpulse() const
+	{
+		return m_maxAppliedImpulse;
+	}
+	void setMaxAppliedImpulse(btScalar maxImp)
+	{
+		m_maxAppliedImpulse = maxImp;
+	}
+
+	virtual void debugDraw(class btIDebugDraw * drawer) = 0;
+
+	virtual void setGearRatio(btScalar ratio) {}
+	virtual void setGearAuxLink(int gearAuxLink) {}
+	virtual void setRelativePositionTarget(btScalar relPosTarget) {}
+	virtual void setErp(btScalar erp) {}
+};
+
+#endif  //BT_MULTIBODY_CONSTRAINT_H

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h

@@ -0,0 +1,101 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H
+#define BT_MULTIBODY_CONSTRAINT_SOLVER_H
+
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "btMultiBodySolverConstraint.h"
+
+#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+
+class btMultiBody;
+
+#include "btMultiBodyConstraint.h"
+
+ATTRIBUTE_ALIGNED16(class)
+btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
+{
+protected:
+	btMultiBodyConstraintArray m_multiBodyNonContactConstraints;
+
+	btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
+	btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
+	btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints;
+	btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints;
+
+	btMultiBodyJacobianData m_data;
+
+	//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
+	btMultiBodyConstraint** m_tmpMultiBodyConstraints;
+	int m_tmpNumMultiBodyConstraints;
+
+	btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
+
+	//solve 2 friction directions and clamp against the implicit friction cone
+	btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB);
+
+	void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
+
+	btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
+																		 btScalar combinedTorsionalFriction,
+																		 btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	btMultiBodySolverConstraint& addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
+		btScalar combinedTorsionalFriction,
+		btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint & constraintRow,
+											btScalar * jacA, btScalar * jacB,
+											btScalar penetration, btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
+											const btContactSolverInfo& infoGlobal);
+
+	void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint,
+										 const btVector3& contactNormal,
+                     const btScalar& appliedImpulse,
+										 btManifoldPoint& cp,
+                     const btContactSolverInfo& infoGlobal,
+										 btScalar& relaxation,
+										 bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	//either rolling or spinning friction
+	void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint & solverConstraint,
+												   const btVector3& contactNormal,
+												   btManifoldPoint& cp,
+												   btScalar combinedTorsionalFriction,
+												   const btContactSolverInfo& infoGlobal,
+												   btScalar& relaxation,
+												   bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	void convertMultiBodyContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+	//	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+	void applyDeltaVee(btScalar * deltaV, btScalar impulse, int velocityIndex, int ndof);
+	void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & constraint, btScalar deltaTime);
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
+	virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
+
+	virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
+};
+
+#endif  //BT_MULTIBODY_CONSTRAINT_SOLVER_H

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -0,0 +1,895 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btMultiBodyDynamicsWorld.h"
+#include "btMultiBodyConstraintSolver.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+#include "LinearMath/btQuickprof.h"
+#include "btMultiBodyConstraint.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btSerializer.h"
+
+void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int mask)
+{
+	m_multiBodies.push_back(body);
+}
+
+void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
+{
+	m_multiBodies.remove(body);
+}
+
+void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+    btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
+    predictMultiBodyTransforms(timeStep);
+    
+}
+void btMultiBodyDynamicsWorld::calculateSimulationIslands()
+{
+	BT_PROFILE("calculateSimulationIslands");
+
+	getSimulationIslandManager()->updateActivationState(getCollisionWorld(), getCollisionWorld()->getDispatcher());
+
+	{
+		//merge islands based on speculative contact manifolds too
+		for (int i = 0; i < this->m_predictiveManifolds.size(); i++)
+		{
+			btPersistentManifold* manifold = m_predictiveManifolds[i];
+
+			const btCollisionObject* colObj0 = manifold->getBody0();
+			const btCollisionObject* colObj1 = manifold->getBody1();
+
+			if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
+				((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
+			{
+				getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
+			}
+		}
+	}
+
+	{
+		int i;
+		int numConstraints = int(m_constraints.size());
+		for (i = 0; i < numConstraints; i++)
+		{
+			btTypedConstraint* constraint = m_constraints[i];
+			if (constraint->isEnabled())
+			{
+				const btRigidBody* colObj0 = &constraint->getRigidBodyA();
+				const btRigidBody* colObj1 = &constraint->getRigidBodyB();
+
+				if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
+					((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
+				{
+					getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
+				}
+			}
+		}
+	}
+
+	//merge islands linked by Featherstone link colliders
+	for (int i = 0; i < m_multiBodies.size(); i++)
+	{
+		btMultiBody* body = m_multiBodies[i];
+		{
+			btMultiBodyLinkCollider* prev = body->getBaseCollider();
+
+			for (int b = 0; b < body->getNumLinks(); b++)
+			{
+				btMultiBodyLinkCollider* cur = body->getLink(b).m_collider;
+
+				if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
+					((prev) && (!(prev)->isStaticOrKinematicObject())))
+				{
+					int tagPrev = prev->getIslandTag();
+					int tagCur = cur->getIslandTag();
+					getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur);
+				}
+				if (cur && !cur->isStaticOrKinematicObject())
+					prev = cur;
+			}
+		}
+	}
+
+	//merge islands linked by multibody constraints
+	{
+		for (int i = 0; i < this->m_multiBodyConstraints.size(); i++)
+		{
+			btMultiBodyConstraint* c = m_multiBodyConstraints[i];
+			int tagA = c->getIslandIdA();
+			int tagB = c->getIslandIdB();
+			if (tagA >= 0 && tagB >= 0)
+				getSimulationIslandManager()->getUnionFind().unite(tagA, tagB);
+		}
+	}
+
+	//Store the island id in each body
+	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
+}
+
+void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
+{
+	BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState");
+
+	for (int i = 0; i < m_multiBodies.size(); i++)
+	{
+		btMultiBody* body = m_multiBodies[i];
+		if (body)
+		{
+			body->checkMotionAndSleepIfRequired(timeStep);
+			if (!body->isAwake())
+			{
+				btMultiBodyLinkCollider* col = body->getBaseCollider();
+				if (col && col->getActivationState() == ACTIVE_TAG)
+				{
+                    if (body->hasFixedBase())
+					{
+                        col->setActivationState(FIXED_BASE_MULTI_BODY);
+                    } else
+					{
+                        col->setActivationState(WANTS_DEACTIVATION);
+                    }
+					
+					col->setDeactivationTime(0.f);
+				}
+				for (int b = 0; b < body->getNumLinks(); b++)
+				{
+					btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
+					if (col && col->getActivationState() == ACTIVE_TAG)
+					{
+						col->setActivationState(WANTS_DEACTIVATION);
+						col->setDeactivationTime(0.f);
+					}
+				}
+			}
+			else
+			{
+				btMultiBodyLinkCollider* col = body->getBaseCollider();
+				if (col && col->getActivationState() != DISABLE_DEACTIVATION)
+					col->setActivationState(ACTIVE_TAG);
+
+				for (int b = 0; b < body->getNumLinks(); b++)
+				{
+					btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
+					if (col && col->getActivationState() != DISABLE_DEACTIVATION)
+						col->setActivationState(ACTIVE_TAG);
+				}
+			}
+		}
+	}
+
+	btDiscreteDynamicsWorld::updateActivationState(timeStep);
+}
+
+void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const
+{
+	islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData;
+}
+
+btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+	: btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
+	  m_multiBodyConstraintSolver(constraintSolver)
+{
+	//split impulse is not yet supported for Featherstone hierarchies
+	//	getSolverInfo().m_splitImpulse = false;
+	getSolverInfo().m_solverMode |= SOLVER_USE_2_FRICTION_DIRECTIONS;
+	m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
+}
+
+btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld()
+{
+	delete m_solverMultiBodyIslandCallback;
+}
+
+void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
+{
+	m_multiBodyConstraintSolver = solver;
+	m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver);
+	btDiscreteDynamicsWorld::setConstraintSolver(solver);
+}
+
+void btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
+{
+	if (solver->getSolverType() == BT_MULTIBODY_SOLVER)
+	{
+		m_multiBodyConstraintSolver = (btMultiBodyConstraintSolver*)solver;
+	}
+	btDiscreteDynamicsWorld::setConstraintSolver(solver);
+}
+
+void btMultiBodyDynamicsWorld::forwardKinematics()
+{
+	for (int b = 0; b < m_multiBodies.size(); b++)
+	{
+		btMultiBody* bod = m_multiBodies[b];
+		bod->forwardKinematics(m_scratch_world_to_local, m_scratch_local_origin);
+	}
+}
+void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
+{
+    solveExternalForces(solverInfo);
+    buildIslands();
+    solveInternalConstraints(solverInfo);
+}
+
+void btMultiBodyDynamicsWorld::buildIslands()
+{
+    m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
+}
+
+void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo)
+{
+	/// solve all the constraints for this island
+	m_solverMultiBodyIslandCallback->processConstraints();
+	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
+    {
+        BT_PROFILE("btMultiBody stepVelocities");
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
+            
+            bool isSleeping = false;
+            
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
+            
+            if (!isSleeping)
+            {
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
+                m_scratch_m.resize(bod->getNumLinks() + 1);
+                
+                if (bod->internalNeedsJointFeedback())
+                {
+                    if (!bod->isUsingRK4Integration())
+                    {
+                        if (bod->internalNeedsJointFeedback())
+                        {
+                            bool isConstraintPass = true;
+                            bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+                                                                                      getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                      getSolverInfo().m_jointFeedbackInJointFrame);
+                        }
+                    }
+                }
+            }
+        }
+    }
+    for (int i = 0; i < this->m_multiBodies.size(); i++)
+    {
+        btMultiBody* bod = m_multiBodies[i];
+        bod->processDeltaVeeMultiDof2();
+    }
+}
+
+void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo)
+{
+    forwardKinematics();
+    
+    BT_PROFILE("solveConstraints");
+    
+    clearMultiBodyConstraintForces();
+    
+    m_sortedConstraints.resize(m_constraints.size());
+    int i;
+    for (i = 0; i < getNumConstraints(); i++)
+    {
+        m_sortedConstraints[i] = m_constraints[i];
+    }
+    m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
+    btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
+    
+    m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
+    for (i = 0; i < m_multiBodyConstraints.size(); i++)
+    {
+        m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
+    }
+    m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
+    
+    btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
+    
+    m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
+    m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
+    
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+    {
+        BT_PROFILE("btMultiBody addForce");
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
+            
+            bool isSleeping = false;
+            
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
+            
+            if (!isSleeping)
+            {
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
+                m_scratch_m.resize(bod->getNumLinks() + 1);
+                
+                bod->addBaseForce(m_gravity * bod->getBaseMass());
+                
+                for (int j = 0; j < bod->getNumLinks(); ++j)
+                {
+                    bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+                }
+            }  //if (!isSleeping)
+        }
+    }
+#endif  //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+    
+    {
+        BT_PROFILE("btMultiBody stepVelocities");
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
+            
+            bool isSleeping = false;
+            
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
+            
+            if (!isSleeping)
+            {
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
+                m_scratch_m.resize(bod->getNumLinks() + 1);
+                bool doNotUpdatePos = false;
+                bool isConstraintPass = false;
+                {
+                    if (!bod->isUsingRK4Integration())
+                    {
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
+                                                                                  m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
+                                                                                  getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                    }
+                    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 &m_scratch_r[bod->getNumDofs()]
+                        //calc qdd0 from: q0 & qd0
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        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->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        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->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        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->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        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->setPosUpdated(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->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                      isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                      getSolverInfo().m_jointFeedbackInJointFrame);
+                        }
+                    }
+                }
+                
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+                bod->clearForcesAndTorques();
+#endif         //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+            }  //if (!isSleeping)
+        }
+    }
+}
+
+
+void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
+{
+	btDiscreteDynamicsWorld::integrateTransforms(timeStep);
+    integrateMultiBodyTransforms(timeStep);
+}
+
+void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
+{
+		BT_PROFILE("btMultiBody stepPositions");
+		//integrate and update the Featherstone hierarchies
+
+		for (int b = 0; b < m_multiBodies.size(); b++)
+		{
+			btMultiBody* bod = m_multiBodies[b];
+			bool isSleeping = false;
+			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+			{
+				isSleeping = true;
+			}
+			for (int b = 0; b < bod->getNumLinks(); b++)
+			{
+				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+					isSleeping = true;
+			}
+
+			if (!isSleeping)
+			{
+				bod->addSplitV();
+				int nLinks = bod->getNumLinks();
+
+				///base + num m_links
+                if (!bod->isPosUpdated())
+                    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->setPosUpdated(false);
+                }
+
+
+				m_scratch_world_to_local.resize(nLinks + 1);
+				m_scratch_local_origin.resize(nLinks + 1);
+                bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+				bod->substractSplitV();
+			}
+			else
+			{
+				bod->clearVelocities();
+			}
+		}
+}
+
+void btMultiBodyDynamicsWorld::predictMultiBodyTransforms(btScalar timeStep)
+{
+    BT_PROFILE("btMultiBody stepPositions");
+    //integrate and update the Featherstone hierarchies
+    
+    for (int b = 0; b < m_multiBodies.size(); b++)
+    {
+        btMultiBody* bod = m_multiBodies[b];
+        bool isSleeping = false;
+        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+        {
+            isSleeping = true;
+        }
+        for (int b = 0; b < bod->getNumLinks(); b++)
+        {
+            if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                isSleeping = true;
+        }
+        
+        if (!isSleeping)
+        {
+            int nLinks = bod->getNumLinks();
+            bod->predictPositionsMultiDof(timeStep);
+            m_scratch_world_to_local.resize(nLinks + 1);
+            m_scratch_local_origin.resize(nLinks + 1);
+            bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+        }
+        else
+        {
+            bod->clearVelocities();
+        }
+    }
+}
+
+void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint)
+{
+	m_multiBodyConstraints.push_back(constraint);
+}
+
+void btMultiBodyDynamicsWorld::removeMultiBodyConstraint(btMultiBodyConstraint* constraint)
+{
+	m_multiBodyConstraints.remove(constraint);
+}
+
+void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
+{
+	constraint->debugDraw(getDebugDrawer());
+}
+
+void btMultiBodyDynamicsWorld::debugDrawWorld()
+{
+	BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
+
+	btDiscreteDynamicsWorld::debugDrawWorld();
+
+	bool drawConstraints = false;
+	if (getDebugDrawer())
+	{
+		int mode = getDebugDrawer()->getDebugMode();
+		if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
+		{
+			drawConstraints = true;
+		}
+
+		if (drawConstraints)
+		{
+			BT_PROFILE("btMultiBody debugDrawWorld");
+
+			for (int c = 0; c < m_multiBodyConstraints.size(); c++)
+			{
+				btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
+				debugDrawMultiBodyConstraint(constraint);
+			}
+
+			for (int b = 0; b < m_multiBodies.size(); b++)
+			{
+				btMultiBody* bod = m_multiBodies[b];
+				bod->forwardKinematics(m_scratch_world_to_local1, m_scratch_local_origin1);
+
+				if (mode & btIDebugDraw::DBG_DrawFrames)
+				{
+					getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
+				}
+
+				for (int m = 0; m < bod->getNumLinks(); m++)
+				{
+					const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
+					if (mode & btIDebugDraw::DBG_DrawFrames)
+					{
+						getDebugDrawer()->drawTransform(tr, 0.1);
+					}
+					//draw the joint axis
+					if (bod->getLink(m).m_jointType == btMultibodyLink::eRevolute)
+					{
+						btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_topVec) * 0.1;
+
+						btVector4 color(0, 0, 0, 1);  //1,1,1);
+						btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						getDebugDrawer()->drawLine(from, to, color);
+					}
+					if (bod->getLink(m).m_jointType == btMultibodyLink::eFixed)
+					{
+						btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1;
+
+						btVector4 color(0, 0, 0, 1);  //1,1,1);
+						btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						getDebugDrawer()->drawLine(from, to, color);
+					}
+					if (bod->getLink(m).m_jointType == btMultibodyLink::ePrismatic)
+					{
+						btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1;
+
+						btVector4 color(0, 0, 0, 1);  //1,1,1);
+						btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						getDebugDrawer()->drawLine(from, to, color);
+					}
+				}
+			}
+		}
+	}
+}
+
+void btMultiBodyDynamicsWorld::applyGravity()
+{
+	btDiscreteDynamicsWorld::applyGravity();
+#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+	BT_PROFILE("btMultiBody addGravity");
+	for (int i = 0; i < this->m_multiBodies.size(); i++)
+	{
+		btMultiBody* bod = m_multiBodies[i];
+
+		bool isSleeping = false;
+
+		if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+		{
+			isSleeping = true;
+		}
+		for (int b = 0; b < bod->getNumLinks(); b++)
+		{
+			if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+				isSleeping = true;
+		}
+
+		if (!isSleeping)
+		{
+			bod->addBaseForce(m_gravity * bod->getBaseMass());
+
+			for (int j = 0; j < bod->getNumLinks(); ++j)
+			{
+				bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+			}
+		}  //if (!isSleeping)
+	}
+#endif  //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+}
+
+void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
+{
+	for (int i = 0; i < this->m_multiBodies.size(); i++)
+	{
+		btMultiBody* bod = m_multiBodies[i];
+		bod->clearConstraintForces();
+	}
+}
+void btMultiBodyDynamicsWorld::clearMultiBodyForces()
+{
+	{
+		// BT_PROFILE("clearMultiBodyForces");
+		for (int i = 0; i < this->m_multiBodies.size(); i++)
+		{
+			btMultiBody* bod = m_multiBodies[i];
+
+			bool isSleeping = false;
+
+			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+			{
+				isSleeping = true;
+			}
+			for (int b = 0; b < bod->getNumLinks(); b++)
+			{
+				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+					isSleeping = true;
+			}
+
+			if (!isSleeping)
+			{
+				btMultiBody* bod = m_multiBodies[i];
+				bod->clearForcesAndTorques();
+			}
+		}
+	}
+}
+void btMultiBodyDynamicsWorld::clearForces()
+{
+	btDiscreteDynamicsWorld::clearForces();
+
+#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+	clearMultiBodyForces();
+#endif
+}
+
+void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
+{
+	serializer->startSerialization();
+
+	serializeDynamicsWorldInfo(serializer);
+
+	serializeMultiBodies(serializer);
+
+	serializeRigidBodies(serializer);
+
+	serializeCollisionObjects(serializer);
+
+	serializeContactManifolds(serializer);
+
+	serializer->finishSerialization();
+}
+
+void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
+{
+	int i;
+	//serialize all collision objects
+	for (i = 0; i < m_multiBodies.size(); i++)
+	{
+		btMultiBody* mb = m_multiBodies[i];
+		{
+			int len = mb->calculateSerializeBufferSize();
+			btChunk* chunk = serializer->allocate(len, 1);
+			const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb);
+		}
+	}
+
+	//serialize all multibody links (collision objects)
+	for (i = 0; i < m_collisionObjects.size(); i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		if (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+		{
+			int len = colObj->calculateSerializeBufferSize();
+			btChunk* chunk = serializer->allocate(len, 1);
+			const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk, structType, BT_MB_LINKCOLLIDER_CODE, colObj);
+		}
+	}
+}
+
+void btMultiBodyDynamicsWorld::saveKinematicState(btScalar timeStep)
+{
+	btDiscreteDynamicsWorld::saveKinematicState(timeStep);
+	for(int i = 0; i < m_multiBodies.size(); i++)
+	{
+		btMultiBody* body = m_multiBodies[i];
+		if(body->isBaseKinematic())
+			body->saveKinematicState(timeStep);
+	}
+}
+
+//
+//void btMultiBodyDynamicsWorld::setSplitIslands(bool split)
+//{
+//    m_islandManager->setSplitIslands(split);
+//}

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -0,0 +1,126 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H
+#define BT_MULTIBODY_DYNAMICS_WORLD_H
+
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h"
+
+#define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+
+class btMultiBody;
+class btMultiBodyConstraint;
+class btMultiBodyConstraintSolver;
+struct MultiBodyInplaceSolverIslandCallback;
+
+///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet
+///This implementation is still preliminary/experimental.
+class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld
+{
+protected:
+	btAlignedObjectArray<btMultiBody*> m_multiBodies;
+	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
+	btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
+	btMultiBodyConstraintSolver* m_multiBodyConstraintSolver;
+	MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback;
+
+	//cached data to avoid memory allocations
+	btAlignedObjectArray<btQuaternion> m_scratch_world_to_local;
+	btAlignedObjectArray<btVector3> m_scratch_local_origin;
+	btAlignedObjectArray<btQuaternion> m_scratch_world_to_local1;
+	btAlignedObjectArray<btVector3> m_scratch_local_origin1;
+	btAlignedObjectArray<btScalar> m_scratch_r;
+	btAlignedObjectArray<btVector3> m_scratch_v;
+	btAlignedObjectArray<btMatrix3x3> m_scratch_m;
+
+	virtual void calculateSimulationIslands();
+	virtual void updateActivationState(btScalar timeStep);
+	
+
+	virtual void serializeMultiBodies(btSerializer* serializer);
+
+public:
+	btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration);
+
+	virtual ~btMultiBodyDynamicsWorld();
+    
+    virtual void solveConstraints(btContactSolverInfo& solverInfo);
+    
+	virtual void addMultiBody(btMultiBody* body, int group = btBroadphaseProxy::DefaultFilter, int mask = btBroadphaseProxy::AllFilter);
+
+	virtual void removeMultiBody(btMultiBody* body);
+
+	virtual int getNumMultibodies() const
+	{
+		return m_multiBodies.size();
+	}
+
+	btMultiBody* getMultiBody(int mbIndex)
+	{
+		return m_multiBodies[mbIndex];
+	}
+
+	const btMultiBody* getMultiBody(int mbIndex) const
+	{
+		return m_multiBodies[mbIndex];
+	}
+
+	virtual void addMultiBodyConstraint(btMultiBodyConstraint* constraint);
+
+	virtual int getNumMultiBodyConstraints() const
+	{
+		return m_multiBodyConstraints.size();
+	}
+
+	virtual btMultiBodyConstraint* getMultiBodyConstraint(int constraintIndex)
+	{
+		return m_multiBodyConstraints[constraintIndex];
+	}
+
+	virtual const btMultiBodyConstraint* getMultiBodyConstraint(int constraintIndex) const
+	{
+		return m_multiBodyConstraints[constraintIndex];
+	}
+
+	virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint);
+
+	virtual void integrateTransforms(btScalar timeStep);
+    void integrateMultiBodyTransforms(btScalar timeStep);
+    void predictMultiBodyTransforms(btScalar timeStep);
+    
+    virtual void predictUnconstraintMotion(btScalar timeStep);
+	virtual void debugDrawWorld();
+
+	virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
+
+	void forwardKinematics();
+	virtual void clearForces();
+	virtual void clearMultiBodyConstraintForces();
+	virtual void clearMultiBodyForces();
+	virtual void applyGravity();
+
+	virtual void serialize(btSerializer* serializer);
+	virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
+	virtual void setConstraintSolver(btConstraintSolver* solver);
+	virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
+    
+    virtual void solveExternalForces(btContactSolverInfo& solverInfo);
+    virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
+    void buildIslands();
+
+	virtual void saveKinematicState(btScalar timeStep);
+};
+#endif  //BT_MULTIBODY_DYNAMICS_WORLD_H

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp

@@ -0,0 +1,215 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyFixedConstraint.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#define BTMBFIXEDCONSTRAINT_DIM 6
+
+btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+	: btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(bodyB),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB)
+{
+	m_data.resize(BTMBFIXEDCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(0),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB)
+{
+	m_data.resize(BTMBFIXEDCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+void btMultiBodyFixedConstraint::finalizeMultiDof()
+{
+	//not implemented yet
+	btAssert(0);
+}
+
+btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
+{
+}
+
+int btMultiBodyFixedConstraint::getIslandIdA() const
+{
+	if (m_rigidBodyA)
+		return m_rigidBodyA->getIslandTag();
+
+	if (m_bodyA)
+	{
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodyFixedConstraint::getIslandIdB() const
+{
+	if (m_rigidBodyB)
+		return m_rigidBodyB->getIslandTag();
+	if (m_bodyB)
+	{
+		if (m_linkB < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
+{
+	int numDim = BTMBFIXEDCONSTRAINT_DIM;
+	for (int i = 0; i < numDim; i++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = i;
+		constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal1.setValue(0, 0, 0);
+		constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal2.setValue(0, 0, 0);
+		constraintRow.m_angularComponentA.setValue(0, 0, 0);
+		constraintRow.m_angularComponentB.setValue(0, 0, 0);
+
+		constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
+		constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
+
+		// Convert local points back to world
+		btVector3 pivotAworld = m_pivotInA;
+		btMatrix3x3 frameAworld = m_frameInA;
+		if (m_rigidBodyA)
+		{
+			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
+			pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+			frameAworld = frameAworld.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
+		}
+		else
+		{
+			if (m_bodyA)
+			{
+				pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+				frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
+			}
+		}
+		btVector3 pivotBworld = m_pivotInB;
+		btMatrix3x3 frameBworld = m_frameInB;
+		if (m_rigidBodyB)
+		{
+			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
+			pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+			frameBworld = frameBworld.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
+		}
+		else
+		{
+			if (m_bodyB)
+			{
+				pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+				frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
+			}
+		}
+
+		btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
+		btVector3 angleDiff;
+		btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
+
+		btVector3 constraintNormalLin(0, 0, 0);
+		btVector3 constraintNormalAng(0, 0, 0);
+		btScalar posError = 0.0;
+		if (i < 3)
+		{
+			constraintNormalLin[i] = 1;
+			posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse);
+		}
+		else
+		{  //i>=3
+			constraintNormalAng = frameAworld.getColumn(i % 3);
+			posError = angleDiff[i % 3];
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
+		}
+	}
+}
+
+void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer)
+{
+	btTransform tr;
+	tr.setIdentity();
+
+	if (m_rigidBodyA)
+	{
+		btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyA)
+	{
+		btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		tr.setOrigin(pivotAworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_rigidBodyB)
+	{
+		// that ideally should draw the same frame
+		btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyB)
+	{
+		btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		tr.setOrigin(pivotBworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+}

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h

@@ -0,0 +1,91 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_FIXED_CONSTRAINT_H
+#define BT_MULTIBODY_FIXED_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+
+class btMultiBodyFixedConstraint : public btMultiBodyConstraint
+{
+protected:
+	btRigidBody* m_rigidBodyA;
+	btRigidBody* m_rigidBodyB;
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
+	btMatrix3x3 m_frameInA;
+	btMatrix3x3 m_frameInB;
+
+public:
+	btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+	btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+
+	virtual ~btMultiBodyFixedConstraint();
+
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	const btVector3& getPivotInA() const
+	{
+		return m_pivotInA;
+	}
+
+	void setPivotInA(const btVector3& pivotInA)
+	{
+		m_pivotInA = pivotInA;
+	}
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	virtual void setPivotInB(const btVector3& pivotInB)
+	{
+		m_pivotInB = pivotInB;
+	}
+
+	const btMatrix3x3& getFrameInA() const
+	{
+		return m_frameInA;
+	}
+
+	void setFrameInA(const btMatrix3x3& frameInA)
+	{
+		m_frameInA = frameInA;
+	}
+
+	const btMatrix3x3& getFrameInB() const
+	{
+		return m_frameInB;
+	}
+
+	virtual void setFrameInB(const btMatrix3x3& frameInB)
+	{
+		m_frameInB = frameInB;
+	}
+
+	virtual void debugDraw(class btIDebugDraw* drawer);
+};
+
+#endif  //BT_MULTIBODY_FIXED_CONSTRAINT_H

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp

@@ -0,0 +1,181 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyGearConstraint.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false, MULTIBODY_CONSTRAINT_GEAR),
+	  m_gearRatio(1),
+	  m_gearAuxLink(-1),
+	  m_erp(0),
+	  m_relativePositionTarget(0)
+{
+}
+
+void btMultiBodyGearConstraint::finalizeMultiDof()
+{
+	allocateJacobiansMultiDof();
+
+	m_numDofsFinalized = m_jacSizeBoth;
+}
+
+btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
+{
+}
+
+int btMultiBodyGearConstraint::getIslandIdA() const
+{
+	if (m_bodyA)
+	{
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodyGearConstraint::getIslandIdB() const
+{
+	if (m_bodyB)
+	{
+		if (m_linkB < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+													 btMultiBodyJacobianData& data,
+													 const btContactSolverInfo& infoGlobal)
+{
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+		finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+
+	if (m_maxAppliedImpulse == 0.f)
+		return;
+
+	// note: we rely on the fact that data.m_jacobians are
+	// always initialized to zero by the Constraint ctor
+	int linkDoF = 0;
+	unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
+	unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF);
+
+	// row 0: the lower bound
+	jacobianA(0)[offsetA] = 1;
+	jacobianB(0)[offsetB] = m_gearRatio;
+
+	btScalar posError = 0;
+	const btVector3 dummy(0, 0, 0);
+
+	btScalar kp = 1;
+	btScalar kd = 1;
+	int numRows = getNumRows();
+
+	for (int row = 0; row < numRows; row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+
+		int dof = 0;
+		btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar auxVel = 0;
+
+		if (m_gearAuxLink >= 0)
+		{
+			auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
+		}
+		currentVelocity += auxVel;
+		if (m_erp != 0)
+		{
+			btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+			if (m_gearAuxLink >= 0)
+			{
+				currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof];
+			}
+			btScalar currentPositionB = m_gearRatio * m_bodyA->getJointPosMultiDof(m_linkB)[dof];
+			btScalar diff = currentPositionB + currentPositionA;
+			btScalar desiredPositionDiff = this->m_relativePositionTarget;
+			posError = -m_erp * (desiredPositionDiff - diff);
+		}
+
+		btScalar desiredRelativeVelocity = auxVel;
+
+		fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, desiredRelativeVelocity);
+
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = row;
+		{
+			//expect either prismatic or revolute joint type for now
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eRevolute:
+				{
+					constraintRow.m_contactNormal1.setZero();
+					constraintRow.m_contactNormal2.setZero();
+					btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
+
+					break;
+				}
+				case btMultibodyLink::ePrismatic:
+				{
+					btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1 = prismaticAxisInWorld;
+					constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
+					constraintRow.m_relpos1CrossNormal.setZero();
+					constraintRow.m_relpos2CrossNormal.setZero();
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+		}
+	}
+}

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h

@@ -0,0 +1,115 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_GEAR_CONSTRAINT_H
+#define BT_MULTIBODY_GEAR_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+
+class btMultiBodyGearConstraint : public btMultiBodyConstraint
+{
+protected:
+	btRigidBody* m_rigidBodyA;
+	btRigidBody* m_rigidBodyB;
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
+	btMatrix3x3 m_frameInA;
+	btMatrix3x3 m_frameInB;
+	btScalar m_gearRatio;
+	int m_gearAuxLink;
+	btScalar m_erp;
+	btScalar m_relativePositionTarget;
+
+public:
+	//btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+	btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+
+	virtual ~btMultiBodyGearConstraint();
+
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	const btVector3& getPivotInA() const
+	{
+		return m_pivotInA;
+	}
+
+	void setPivotInA(const btVector3& pivotInA)
+	{
+		m_pivotInA = pivotInA;
+	}
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	virtual void setPivotInB(const btVector3& pivotInB)
+	{
+		m_pivotInB = pivotInB;
+	}
+
+	const btMatrix3x3& getFrameInA() const
+	{
+		return m_frameInA;
+	}
+
+	void setFrameInA(const btMatrix3x3& frameInA)
+	{
+		m_frameInA = frameInA;
+	}
+
+	const btMatrix3x3& getFrameInB() const
+	{
+		return m_frameInB;
+	}
+
+	virtual void setFrameInB(const btMatrix3x3& frameInB)
+	{
+		m_frameInB = frameInB;
+	}
+
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
+
+	virtual void setGearRatio(btScalar gearRatio)
+	{
+		m_gearRatio = gearRatio;
+	}
+	virtual void setGearAuxLink(int gearAuxLink)
+	{
+		m_gearAuxLink = gearAuxLink;
+	}
+	virtual void setRelativePositionTarget(btScalar relPosTarget)
+	{
+		m_relativePositionTarget = relPosTarget;
+	}
+	virtual void setErp(btScalar erp)
+	{
+		m_erp = erp;
+	}
+};
+
+#endif  //BT_MULTIBODY_GEAR_CONSTRAINT_H

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h

@@ -0,0 +1,247 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H
+#define BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H
+
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "btMultiBodyConstraintSolver.h"
+
+SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs)
+{
+    int islandId;
+    
+    const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
+    const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
+    islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
+    return islandId;
+}
+class btSortConstraintOnIslandPredicate2
+{
+public:
+    bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const
+    {
+        int rIslandId0, lIslandId0;
+        rIslandId0 = btGetConstraintIslandId2(rhs);
+        lIslandId0 = btGetConstraintIslandId2(lhs);
+        return lIslandId0 < rIslandId0;
+    }
+};
+
+SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
+{
+    int islandId;
+    
+    int islandTagA = lhs->getIslandIdA();
+    int islandTagB = lhs->getIslandIdB();
+    islandId = islandTagA >= 0 ? islandTagA : islandTagB;
+    return islandId;
+}
+
+class btSortMultiBodyConstraintOnIslandPredicate
+{
+public:
+    bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const
+    {
+        int rIslandId0, lIslandId0;
+        rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
+        lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
+        return lIslandId0 < rIslandId0;
+    }
+};
+
+struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
+{
+
+    btContactSolverInfo* m_solverInfo;
+    btMultiBodyConstraintSolver* m_solver;
+    btMultiBodyConstraint** m_multiBodySortedConstraints;
+    int m_numMultiBodyConstraints;
+    
+    btTypedConstraint** m_sortedConstraints;
+    int m_numConstraints;
+    btIDebugDraw* m_debugDrawer;
+    btDispatcher* m_dispatcher;
+    
+    btAlignedObjectArray<btCollisionObject*> m_bodies;
+	btAlignedObjectArray<btCollisionObject*> m_softBodies;
+    btAlignedObjectArray<btPersistentManifold*> m_manifolds;
+    btAlignedObjectArray<btTypedConstraint*> m_constraints;
+    btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
+    
+    btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData;
+    
+    MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver,
+                                         btDispatcher* dispatcher)
+    : m_solverInfo(NULL),
+    m_solver(solver),
+    m_multiBodySortedConstraints(NULL),
+    m_numConstraints(0),
+    m_debugDrawer(NULL),
+    m_dispatcher(dispatcher)
+    {
+    }
+    
+    MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other)
+    {
+        btAssert(0);
+        (void)other;
+        return *this;
+    }
+    
+    SIMD_FORCE_INLINE virtual void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
+    {
+        m_islandAnalyticsData.clear();
+        btAssert(solverInfo);
+        m_solverInfo = solverInfo;
+        
+        m_multiBodySortedConstraints = sortedMultiBodyConstraints;
+        m_numMultiBodyConstraints = numMultiBodyConstraints;
+        m_sortedConstraints = sortedConstraints;
+        m_numConstraints = numConstraints;
+        
+        m_debugDrawer = debugDrawer;
+        m_bodies.resize(0);
+        m_manifolds.resize(0);
+        m_constraints.resize(0);
+        m_multiBodyConstraints.resize(0);
+    }
+    
+    void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
+    {
+        m_solver = solver;
+    }
+    
+    virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId)
+    {
+        if (islandId < 0)
+        {
+            ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
+            m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
+            if (m_solverInfo->m_reportSolverAnalytics&1)
+            {
+                m_solver->m_analyticsData.m_islandId = islandId;
+                m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+            }
+        }
+        else
+        {
+            //also add all non-contact constraints/joints for this island
+            btTypedConstraint** startConstraint = 0;
+            btMultiBodyConstraint** startMultiBodyConstraint = 0;
+            
+            int numCurConstraints = 0;
+            int numCurMultiBodyConstraints = 0;
+            
+            int i;
+            
+            //find the first constraint for this island
+            
+            for (i = 0; i < m_numConstraints; i++)
+            {
+                if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
+                {
+                    startConstraint = &m_sortedConstraints[i];
+                    break;
+                }
+            }
+            //count the number of constraints in this island
+            for (; i < m_numConstraints; i++)
+            {
+                if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
+                {
+                    numCurConstraints++;
+                }
+            }
+            
+            for (i = 0; i < m_numMultiBodyConstraints; i++)
+            {
+                if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
+                {
+                    startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
+                    break;
+                }
+            }
+            //count the number of multi body constraints in this island
+            for (; i < m_numMultiBodyConstraints; i++)
+            {
+                if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
+                {
+                    numCurMultiBodyConstraints++;
+                }
+            }
+            
+            //if (m_solverInfo->m_minimumSolverBatchSize<=1)
+            //{
+            //    m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
+            //} else
+            {
+                for (i = 0; i < numBodies; i++)
+				{
+					bool isSoftBodyType = (bodies[i]->getInternalType() & btCollisionObject::CO_SOFT_BODY);
+					if (!isSoftBodyType)
+					{
+						m_bodies.push_back(bodies[i]);
+					}
+					else
+					{
+						m_softBodies.push_back(bodies[i]);
+					}
+				}
+                for (i = 0; i < numManifolds; i++)
+                    m_manifolds.push_back(manifolds[i]);
+                for (i = 0; i < numCurConstraints; i++)
+                    m_constraints.push_back(startConstraint[i]);
+                
+                for (i = 0; i < numCurMultiBodyConstraints; i++)
+                    m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
+                
+                if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
+                {
+                    processConstraints(islandId);
+                }
+                else
+                {
+                    //printf("deferred\n");
+                }
+            }
+        }
+    }
+    
+    virtual void processConstraints(int islandId=-1)
+    {
+        btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
+        btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
+        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);
+        if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1))
+        {
+            m_solver->m_analyticsData.m_islandId = islandId;
+            m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+        }
+        m_bodies.resize(0);
+		m_softBodies.resize(0);
+        m_manifolds.resize(0);
+        m_constraints.resize(0);
+        m_multiBodyConstraints.resize(0);
+    }
+};
+
+
+#endif /*BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H */

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h

@@ -0,0 +1,25 @@
+/*
+Copyright (c) 2015 Google Inc.
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_JOINT_FEEDBACK_H
+#define BT_MULTIBODY_JOINT_FEEDBACK_H
+
+#include "LinearMath/btSpatialAlgebra.h"
+
+struct btMultiBodyJointFeedback
+{
+	btSpatialForceVector m_reactionForces;
+};
+
+#endif  //BT_MULTIBODY_JOINT_FEEDBACK_H

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -0,0 +1,197 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyJointLimitConstraint.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
+	//:btMultiBodyConstraint(body,0,link,-1,2,true),
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true, MULTIBODY_CONSTRAINT_LIMIT),
+	  m_lowerBound(lower),
+	  m_upperBound(upper)
+{
+}
+
+void btMultiBodyJointLimitConstraint::finalizeMultiDof()
+{
+	// the data.m_jacobians never change, so may as well
+	// initialize them here
+
+	allocateJacobiansMultiDof();
+
+	unsigned int offset = 6 + m_bodyA->getLink(m_linkA).m_dofOffset;
+
+	// row 0: the lower bound
+	jacobianA(0)[offset] = 1;
+	// row 1: the upper bound
+	//jacobianA(1)[offset] = -1;
+	jacobianB(1)[offset] = -1;
+
+	m_numDofsFinalized = m_jacSizeBoth;
+}
+
+btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
+{
+}
+
+int btMultiBodyJointLimitConstraint::getIslandIdA() const
+{
+	if (m_bodyA)
+	{
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodyJointLimitConstraint::getIslandIdB() const
+{
+	if (m_bodyB)
+	{
+		if (m_linkB < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+														   btMultiBodyJacobianData& data,
+														   const btContactSolverInfo& infoGlobal)
+{
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+		finalizeMultiDof();
+	}
+
+	// row 0: the lower bound
+	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));
+
+	for (int row = 0; row < getNumRows(); row++)
+	{
+		btScalar penetration = getPosition(row);
+
+		//todo: consider adding some safety threshold here
+		if (penetration > 0)
+		{
+			continue;
+		}
+		btScalar direction = row ? -1 : 1;
+
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = row;
+
+		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 = fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, 0, m_maxAppliedImpulse);
+
+		{
+			//expect either prismatic or revolute joint type for now
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eRevolute:
+				{
+					constraintRow.m_contactNormal1.setZero();
+					constraintRow.m_contactNormal2.setZero();
+					btVector3 revoluteAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
+
+					break;
+				}
+				case btMultibodyLink::ePrismatic:
+				{
+					btVector3 prismaticAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1 = prismaticAxisInWorld;
+					constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
+					constraintRow.m_relpos1CrossNormal.setZero();
+					constraintRow.m_relpos2CrossNormal.setZero();
+
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+		}
+
+		{
+			btScalar positionalError = 0.f;
+			btScalar velocityError = -rel_vel;  // * damping;
+			btScalar erp = infoGlobal.m_erp2;
+			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+			{
+				erp = infoGlobal.m_erp;
+			}
+			if (penetration > 0)
+			{
+				positionalError = 0;
+				velocityError = -penetration / infoGlobal.m_timeStep;
+			}
+			else
+			{
+				positionalError = -penetration * erp / infoGlobal.m_timeStep;
+			}
+
+			btScalar penetrationImpulse = positionalError * constraintRow.m_jacDiagABInv;
+			btScalar velocityImpulse = velocityError * constraintRow.m_jacDiagABInv;
+			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+			{
+				//combine position and velocity into rhs
+				constraintRow.m_rhs = penetrationImpulse + velocityImpulse;
+				constraintRow.m_rhsPenetration = 0.f;
+			}
+			else
+			{
+				//split position and velocity into rhs and m_rhsPenetration
+				constraintRow.m_rhs = velocityImpulse;
+				constraintRow.m_rhsPenetration = penetrationImpulse;
+			}
+		}
+	}
+}

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h

@@ -0,0 +1,63 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
+#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+struct btSolverInfo;
+
+class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
+{
+protected:
+	btScalar m_lowerBound;
+	btScalar m_upperBound;
+
+public:
+	btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
+	virtual ~btMultiBodyJointLimitConstraint();
+
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
+	btScalar getLowerBound() const
+	{
+		return m_lowerBound;
+	}
+	btScalar getUpperBound() const
+	{
+		return m_upperBound;
+	}
+	void setLowerBound(btScalar lower)
+	{
+		m_lowerBound = lower;
+	}
+	void setUpperBound(btScalar upper)
+	{
+		m_upperBound = upper;
+	}
+};
+
+#endif  //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -0,0 +1,183 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyJointMotor.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
+	  m_desiredVelocity(desiredVelocity),
+	  m_desiredPosition(0),
+	  m_kd(1.),
+	  m_kp(0),
+	  m_erp(1),
+	  m_rhsClamp(SIMD_INFINITY)
+{
+	m_maxAppliedImpulse = maxMotorImpulse;
+	// the data.m_jacobians never change, so may as well
+	// initialize them here
+}
+
+void btMultiBodyJointMotor::finalizeMultiDof()
+{
+	allocateJacobiansMultiDof();
+	// note: we rely on the fact that data.m_jacobians are
+	// always initialized to zero by the Constraint ctor
+	int linkDoF = 0;
+	unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
+
+	// row 0: the lower bound
+	// row 0: the lower bound
+	jacobianA(0)[offset] = 1;
+
+	m_numDofsFinalized = m_jacSizeBoth;
+}
+
+btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
+	//:btMultiBodyConstraint(body,0,link,-1,1,true),
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
+	  m_desiredVelocity(desiredVelocity),
+	  m_desiredPosition(0),
+	  m_kd(1.),
+	  m_kp(0),
+	  m_erp(1),
+	  m_rhsClamp(SIMD_INFINITY)
+{
+	btAssert(linkDoF < body->getLink(link).m_dofCount);
+
+	m_maxAppliedImpulse = maxMotorImpulse;
+}
+btMultiBodyJointMotor::~btMultiBodyJointMotor()
+{
+}
+
+int btMultiBodyJointMotor::getIslandIdA() const
+{
+	if (this->m_linkA < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyA->getLink(m_linkA).m_collider)
+		{
+			return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodyJointMotor::getIslandIdB() const
+{
+	if (m_linkB < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyB->getLink(m_linkB).m_collider)
+		{
+			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+												 btMultiBodyJacobianData& data,
+												 const btContactSolverInfo& infoGlobal)
+{
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+		finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+
+	if (m_maxAppliedImpulse == 0.f)
+		return;
+
+	const btScalar posError = 0;
+	const btVector3 dummy(0, 0, 0);
+
+	for (int row = 0; row < getNumRows(); row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+
+		int dof = 0;
+		btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar positionStabiliationTerm = m_erp * (m_desiredPosition - currentPosition) / infoGlobal.m_timeStep;
+
+		btScalar velocityError = (m_desiredVelocity - currentVelocity);
+		btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity + m_kd * velocityError;
+		if (rhs > m_rhsClamp)
+		{
+			rhs = m_rhsClamp;
+		}
+		if (rhs < -m_rhsClamp)
+		{
+			rhs = -m_rhsClamp;
+		}
+
+		fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, rhs);
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = row;
+		{
+			//expect either prismatic or revolute joint type for now
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eRevolute:
+				{
+					constraintRow.m_contactNormal1.setZero();
+					constraintRow.m_contactNormal2.setZero();
+					btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
+
+					break;
+				}
+				case btMultibodyLink::ePrismatic:
+				{
+					btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1 = prismaticAxisInWorld;
+					constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
+					constraintRow.m_relpos1CrossNormal.setZero();
+					constraintRow.m_relpos2CrossNormal.setZero();
+
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+		}
+	}
+}

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h

@@ -0,0 +1,77 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_JOINT_MOTOR_H
+#define BT_MULTIBODY_JOINT_MOTOR_H
+
+#include "btMultiBodyConstraint.h"
+struct btSolverInfo;
+
+class btMultiBodyJointMotor : public btMultiBodyConstraint
+{
+protected:
+	btScalar m_desiredVelocity;
+	btScalar m_desiredPosition;
+	btScalar m_kd;
+	btScalar m_kp;
+	btScalar m_erp;
+	btScalar m_rhsClamp;  //maximum error
+
+public:
+	btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
+	btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
+	virtual ~btMultiBodyJointMotor();
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = kd;
+	}
+
+	virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
+	{
+		m_desiredPosition = posTarget;
+		m_kp = kp;
+	}
+
+	virtual void setErp(btScalar erp)
+	{
+		m_erp = erp;
+	}
+	virtual btScalar getErp() const
+	{
+		return m_erp;
+	}
+	virtual void setRhsClamp(btScalar rhsClamp)
+	{
+		m_rhsClamp = rhsClamp;
+	}
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
+};
+
+#endif  //BT_MULTIBODY_JOINT_MOTOR_H

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -0,0 +1,303 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_LINK_H
+#define BT_MULTIBODY_LINK_H
+
+#include "LinearMath/btQuaternion.h"
+#include "LinearMath/btVector3.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+enum btMultiBodyLinkFlags
+{
+	BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,
+	BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,
+};
+
+//both defines are now permanently enabled
+#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+#define TEST_SPATIAL_ALGEBRA_LAYER
+
+//
+// Various spatial helper functions
+//
+
+//namespace {
+
+#include "LinearMath/btSpatialAlgebra.h"
+
+//}
+
+//
+// Link struct
+//
+
+struct btMultibodyLink
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btScalar m_mass;           // mass of link
+	btVector3 m_inertiaLocal;  // inertia of link (local frame; diagonal)
+
+	int m_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.
+
+	btVector3 m_dVector;  // vector from the inboard joint pos to this link's COM. (local frame.) constant.
+						  //this is set to zero for planar joint (see also m_eVector comment)
+
+	// m_eVector is constant, but depends on the joint type:
+	// revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
+	// planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
+	// todo: fix the planar so it is consistent with the other joints
+
+	btVector3 m_eVector;
+
+	btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
+
+	enum eFeatherstoneJointType
+	{
+		eRevolute = 0,
+		ePrismatic = 1,
+		eSpherical = 2,
+		ePlanar = 3,
+		eFixed = 4,
+		eInvalid
+	};
+
+	// "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
+	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; }
+
+	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.
+    
+    // predicted verstion
+    btQuaternion m_cachedRotParentToThis_interpolate;  // rotates vectors in parent frame to vectors in local frame
+    btVector3 m_cachedRVector_interpolate;             // vector from COM of parent to COM of this link, in local frame.
+
+	btVector3 m_appliedForce;   // In WORLD frame
+	btVector3 m_appliedTorque;  // In WORLD frame
+
+	btVector3 m_appliedConstraintForce;   // In WORLD frame
+	btVector3 m_appliedConstraintTorque;  // In WORLD frame
+
+	btScalar m_jointPos[7];
+    btScalar m_jointPos_interpolate[7];
+
+	//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
+	//It gets set to zero after each internal stepSimulation call
+	btScalar m_jointTorque[6];
+
+	class btMultiBodyLinkCollider *m_collider;
+	int m_flags;
+
+	int m_dofCount, m_posVarCount;  //redundant but handy
+
+	eFeatherstoneJointType m_jointType;
+
+	struct btMultiBodyJointFeedback *m_jointFeedback;
+
+	btTransform m_cachedWorldTransform;  //this cache is updated when calling btMultiBody::forwardKinematics
+
+	const char *m_linkName;   //m_linkName memory needs to be managed by the developer/user!
+	const char *m_jointName;  //m_jointName memory needs to be managed by the developer/user!
+	const void *m_userPtr;    //m_userPtr ptr needs to be managed by the developer/user!
+
+	btScalar m_jointDamping;      //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
+	btScalar m_jointFriction;     //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
+	btScalar m_jointLowerLimit;   //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+	btScalar m_jointUpperLimit;   //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+	btScalar m_jointMaxForce;     //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+	btScalar m_jointMaxVelocity;  //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+
+	// ctor: set some sensible defaults
+	btMultibodyLink()
+		: m_mass(1),
+		  m_parent(-1),
+		  m_zeroRotParentToThis(0, 0, 0, 1),
+		  m_cachedRotParentToThis(0, 0, 0, 1),
+          m_cachedRotParentToThis_interpolate(0, 0, 0, 1),
+		  m_collider(0),
+		  m_flags(0),
+		  m_dofCount(0),
+		  m_posVarCount(0),
+		  m_jointType(btMultibodyLink::eInvalid),
+		  m_jointFeedback(0),
+		  m_linkName(0),
+		  m_jointName(0),
+		  m_userPtr(0),
+		  m_jointDamping(0),
+		  m_jointFriction(0),
+		  m_jointLowerLimit(0),
+		  m_jointUpperLimit(0),
+		  m_jointMaxForce(0),
+		  m_jointMaxVelocity(0)
+	{
+		m_inertiaLocal.setValue(1, 1, 1);
+		setAxisTop(0, 0., 0., 0.);
+		setAxisBottom(0, 1., 0., 0.);
+		m_dVector.setValue(0, 0, 0);
+		m_eVector.setValue(0, 0, 0);
+		m_cachedRVector.setValue(0, 0, 0);
+        m_cachedRVector_interpolate.setValue(0, 0, 0);
+		m_appliedForce.setValue(0, 0, 0);
+		m_appliedTorque.setValue(0, 0, 0);
+		m_appliedConstraintForce.setValue(0, 0, 0);
+		m_appliedConstraintTorque.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;
+		m_cachedWorldTransform.setIdentity();
+	}
+
+	// routine to update m_cachedRotParentToThis and m_cachedRVector
+	void updateCacheMultiDof(btScalar *pq = 0)
+	{
+        btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
+        btQuaternion& cachedRot = m_cachedRotParentToThis;
+        btVector3& cachedVector = m_cachedRVector;
+		switch (m_jointType)
+		{
+			case eRevolute:
+			{
+				cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+
+				break;
+			}
+			case ePrismatic:
+			{
+				// m_cachedRotParentToThis never changes, so no need to update
+				cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
+
+				break;
+			}
+			case eSpherical:
+			{
+				cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
+
+				break;
+			}
+			case ePlanar:
+			{
+				cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+				cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
+
+				break;
+			}
+			case eFixed:
+			{
+				cachedRot = m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
+
+				break;
+			}
+			default:
+			{
+				//invalid type
+				btAssert(0);
+			}
+		}
+        m_cachedRotParentToThis_interpolate = m_cachedRotParentToThis;
+        m_cachedRVector_interpolate = m_cachedRVector;
+	}
+    
+    void updateInterpolationCacheMultiDof()
+    {
+        btScalar *pJointPos = &m_jointPos_interpolate[0];
+        
+        btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate;
+        btVector3& cachedVector = m_cachedRVector_interpolate;
+        switch (m_jointType)
+        {
+            case eRevolute:
+            {
+                cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+                
+                break;
+            }
+            case ePrismatic:
+            {
+                // m_cachedRotParentToThis never changes, so no need to update
+                cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
+                
+                break;
+            }
+            case eSpherical:
+            {
+                cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            case ePlanar:
+            {
+                cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+                cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            case eFixed:
+            {
+                cachedRot = m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            default:
+            {
+                //invalid type
+                btAssert(0);
+            }
+        }
+    }
+
+ 
+
+};
+
+#endif  //BT_MULTIBODY_LINK_H

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h

@@ -0,0 +1,195 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H
+#define BT_FEATHERSTONE_LINK_COLLIDER_H
+
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+#include "btMultiBody.h"
+#include "LinearMath/btSerializer.h"
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btMultiBodyLinkColliderData btMultiBodyLinkColliderDoubleData
+#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderDoubleData"
+#else
+#define btMultiBodyLinkColliderData btMultiBodyLinkColliderFloatData
+#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderFloatData"
+#endif
+
+class btMultiBodyLinkCollider : public btCollisionObject
+{
+	//protected:
+public:
+	btMultiBody* m_multiBody;
+	int m_link;
+
+	virtual ~btMultiBodyLinkCollider()
+	{
+
+	}
+	btMultiBodyLinkCollider(btMultiBody* multiBody, int link)
+		: m_multiBody(multiBody),
+		  m_link(link)
+	{
+		m_checkCollideWith = true;
+		//we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands
+		//this means that some constraints might point to bodies that are not in the islands, causing crashes
+		//if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
+		{
+			m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
+		}
+		// else
+		//{
+		//	m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT);
+		//}
+
+		m_internalType = CO_FEATHERSTONE_LINK;
+	}
+	static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK)
+			return (btMultiBodyLinkCollider*)colObj;
+		return 0;
+	}
+	static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK)
+			return (btMultiBodyLinkCollider*)colObj;
+		return 0;
+	}
+
+	virtual bool checkCollideWithOverride(const btCollisionObject* co) const
+	{
+		const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
+		if (!other)
+			return true;
+		if (other->m_multiBody != this->m_multiBody)
+			return true;
+		if (!m_multiBody->hasSelfCollision())
+			return false;
+
+		//check if 'link' has collision disabled
+		if (m_link >= 0)
+		{
+			const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
+			if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
+			{
+				int parent_of_this = m_link;
+				while (1)
+				{
+					if (parent_of_this == -1)
+						break;
+					parent_of_this = m_multiBody->getLink(parent_of_this).m_parent;
+					if (parent_of_this == other->m_link)
+					{
+						return false;
+					}
+				}
+			}
+			else if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
+			{
+				if (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_ALL_PARENT_COLLISION)
+			{
+				int parent_of_other = other->m_link;
+				while (1)
+				{
+					if (parent_of_other == -1)
+						break;
+					parent_of_other = m_multiBody->getLink(parent_of_other).m_parent;
+					if (parent_of_other == this->m_link)
+						return false;
+				}
+			}
+			else if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
+			{
+				if (otherLink.m_parent == this->m_link)
+					return false;
+			}
+		}
+		return true;
+	}
+
+	bool isStaticOrKinematic() const
+	{
+		return isStaticOrKinematicObject();
+	}
+
+	bool isKinematic() const
+	{
+		return isKinematicObject();
+	}
+
+	void setDynamicType(int dynamicType)
+	{
+		int oldFlags = getCollisionFlags();
+		oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT);
+		setCollisionFlags(oldFlags | dynamicType);
+	}
+
+	virtual int calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
+};
+
+// clang-format off
+
+struct	btMultiBodyLinkColliderFloatData
+{
+	btCollisionObjectFloatData m_colObjData;
+	btMultiBodyFloatData	*m_multiBody;
+	int			m_link;
+	char		m_padding[4];
+};
+
+struct	btMultiBodyLinkColliderDoubleData
+{
+	btCollisionObjectDoubleData m_colObjData;
+	btMultiBodyDoubleData		*m_multiBody;
+	int			m_link;
+	char		m_padding[4];
+};
+
+// clang-format on
+
+SIMD_FORCE_INLINE int btMultiBodyLinkCollider::calculateSerializeBufferSize() const
+{
+	return sizeof(btMultiBodyLinkColliderData);
+}
+
+SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffer, class btSerializer* serializer) const
+{
+	btMultiBodyLinkColliderData* dataOut = (btMultiBodyLinkColliderData*)dataBuffer;
+	btCollisionObject::serialize(&dataOut->m_colObjData, serializer);
+
+	dataOut->m_link = this->m_link;
+	dataOut->m_multiBody = (btMultiBodyData*)serializer->getUniquePointer(m_multiBody);
+
+	// Fill padding with zeros to appease msan.
+	memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));
+
+	return btMultiBodyLinkColliderDataName;
+}
+
+#endif  //BT_FEATHERSTONE_LINK_COLLIDER_H

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp

@@ -0,0 +1,966 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
+
+#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+
+static bool interleaveContactAndFriction1 = false;
+
+struct btJointNode1
+{
+	int jointIndex;          // pointer to enclosing dxJoint object
+	int otherBodyIndex;      // *other* body this joint is connected to
+	int nextJointNodeIndex;  //-1 for null
+	int constraintRowIndex;
+};
+
+// Helper function to compute a delta velocity in the constraint space.
+static btScalar computeDeltaVelocityInConstraintSpace(
+	const btVector3& angularDeltaVelocity,
+	const btVector3& contactNormal,
+	btScalar invMass,
+	const btVector3& angularJacobian,
+	const btVector3& linearJacobian)
+{
+	return angularDeltaVelocity.dot(angularJacobian) + contactNormal.dot(linearJacobian) * invMass;
+}
+
+// Faster version of computeDeltaVelocityInConstraintSpace that can be used when contactNormal and linearJacobian are
+// identical.
+static btScalar computeDeltaVelocityInConstraintSpace(
+	const btVector3& angularDeltaVelocity,
+	btScalar invMass,
+	const btVector3& angularJacobian)
+{
+	return angularDeltaVelocity.dot(angularJacobian) + invMass;
+}
+
+// Helper function to compute a delta velocity in the constraint space.
+static btScalar computeDeltaVelocityInConstraintSpace(const btScalar* deltaVelocity, const btScalar* jacobian, int size)
+{
+	btScalar result = 0;
+	for (int i = 0; i < size; ++i)
+		result += deltaVelocity[i] * jacobian[i];
+
+	return result;
+}
+
+static btScalar computeConstraintMatrixDiagElementMultiBody(
+	const btAlignedObjectArray<btSolverBody>& solverBodyPool,
+	const btMultiBodyJacobianData& data,
+	const btMultiBodySolverConstraint& constraint)
+{
+	btScalar ret = 0;
+
+	const btMultiBody* multiBodyA = constraint.m_multiBodyA;
+	const btMultiBody* multiBodyB = constraint.m_multiBodyB;
+
+	if (multiBodyA)
+	{
+		const btScalar* jacA = &data.m_jacobians[constraint.m_jacAindex];
+		const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+		const int ndofA = multiBodyA->getNumDofs() + 6;
+		ret += computeDeltaVelocityInConstraintSpace(deltaA, jacA, ndofA);
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		btAssert(solverBodyIdA != -1);
+		const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+		const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+		ret += computeDeltaVelocityInConstraintSpace(
+			constraint.m_relpos1CrossNormal,
+			invMassA,
+			constraint.m_angularComponentA);
+	}
+
+	if (multiBodyB)
+	{
+		const btScalar* jacB = &data.m_jacobians[constraint.m_jacBindex];
+		const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+		const int ndofB = multiBodyB->getNumDofs() + 6;
+		ret += computeDeltaVelocityInConstraintSpace(deltaB, jacB, ndofB);
+	}
+	else
+	{
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+		btAssert(solverBodyIdB != -1);
+		const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+		const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+		ret += computeDeltaVelocityInConstraintSpace(
+			constraint.m_relpos2CrossNormal,
+			invMassB,
+			constraint.m_angularComponentB);
+	}
+
+	return ret;
+}
+
+static btScalar computeConstraintMatrixOffDiagElementMultiBody(
+	const btAlignedObjectArray<btSolverBody>& solverBodyPool,
+	const btMultiBodyJacobianData& data,
+	const btMultiBodySolverConstraint& constraint,
+	const btMultiBodySolverConstraint& offDiagConstraint)
+{
+	btScalar offDiagA = btScalar(0);
+
+	const btMultiBody* multiBodyA = constraint.m_multiBodyA;
+	const btMultiBody* multiBodyB = constraint.m_multiBodyB;
+	const btMultiBody* offDiagMultiBodyA = offDiagConstraint.m_multiBodyA;
+	const btMultiBody* offDiagMultiBodyB = offDiagConstraint.m_multiBodyB;
+
+	// Assumed at least one system is multibody
+	btAssert(multiBodyA || multiBodyB);
+	btAssert(offDiagMultiBodyA || offDiagMultiBodyB);
+
+	if (offDiagMultiBodyA)
+	{
+		const btScalar* offDiagJacA = &data.m_jacobians[offDiagConstraint.m_jacAindex];
+
+		if (offDiagMultiBodyA == multiBodyA)
+		{
+			const int ndofA = multiBodyA->getNumDofs() + 6;
+			const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacA, ndofA);
+		}
+		else if (offDiagMultiBodyA == multiBodyB)
+		{
+			const int ndofB = multiBodyB->getNumDofs() + 6;
+			const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacA, ndofB);
+		}
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+
+		const int offDiagSolverBodyIdA = offDiagConstraint.m_solverBodyIdA;
+		btAssert(offDiagSolverBodyIdA != -1);
+
+		if (offDiagSolverBodyIdA == solverBodyIdA)
+		{
+			btAssert(solverBodyIdA != -1);
+			const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+			const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos1CrossNormal,
+				offDiagConstraint.m_contactNormal1,
+				invMassA, constraint.m_angularComponentA,
+				constraint.m_contactNormal1);
+		}
+		else if (offDiagSolverBodyIdA == solverBodyIdB)
+		{
+			btAssert(solverBodyIdB != -1);
+			const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+			const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos1CrossNormal,
+				offDiagConstraint.m_contactNormal1,
+				invMassB,
+				constraint.m_angularComponentB,
+				constraint.m_contactNormal2);
+		}
+	}
+
+	if (offDiagMultiBodyB)
+	{
+		const btScalar* offDiagJacB = &data.m_jacobians[offDiagConstraint.m_jacBindex];
+
+		if (offDiagMultiBodyB == multiBodyA)
+		{
+			const int ndofA = multiBodyA->getNumDofs() + 6;
+			const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacB, ndofA);
+		}
+		else if (offDiagMultiBodyB == multiBodyB)
+		{
+			const int ndofB = multiBodyB->getNumDofs() + 6;
+			const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacB, ndofB);
+		}
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+
+		const int offDiagSolverBodyIdB = offDiagConstraint.m_solverBodyIdB;
+		btAssert(offDiagSolverBodyIdB != -1);
+
+		if (offDiagSolverBodyIdB == solverBodyIdA)
+		{
+			btAssert(solverBodyIdA != -1);
+			const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+			const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos2CrossNormal,
+				offDiagConstraint.m_contactNormal2,
+				invMassA, constraint.m_angularComponentA,
+				constraint.m_contactNormal1);
+		}
+		else if (offDiagSolverBodyIdB == solverBodyIdB)
+		{
+			btAssert(solverBodyIdB != -1);
+			const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+			const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos2CrossNormal,
+				offDiagConstraint.m_contactNormal2,
+				invMassB, constraint.m_angularComponentB,
+				constraint.m_contactNormal2);
+		}
+	}
+
+	return offDiagA;
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
+{
+	createMLCPFastRigidBody(infoGlobal);
+	createMLCPFastMultiBody(infoGlobal);
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal)
+{
+	int numContactRows = interleaveContactAndFriction1 ? 3 : 1;
+
+	int numConstraintRows = m_allConstraintPtrArray.size();
+
+	if (numConstraintRows == 0)
+		return;
+
+	int n = numConstraintRows;
+	{
+		BT_PROFILE("init b (rhs)");
+		m_b.resize(numConstraintRows);
+		m_bSplit.resize(numConstraintRows);
+		m_b.setZero();
+		m_bSplit.setZero();
+		for (int i = 0; i < numConstraintRows; i++)
+		{
+			btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
+			if (!btFuzzyZero(jacDiag))
+			{
+				btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
+				btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
+				m_b[i] = rhs / jacDiag;
+				m_bSplit[i] = rhsPenetration / jacDiag;
+			}
+		}
+	}
+
+	//	btScalar* w = 0;
+	//	int nub = 0;
+
+	m_lo.resize(numConstraintRows);
+	m_hi.resize(numConstraintRows);
+
+	{
+		BT_PROFILE("init lo/ho");
+
+		for (int i = 0; i < numConstraintRows; i++)
+		{
+			if (0)  //m_limitDependencies[i]>=0)
+			{
+				m_lo[i] = -BT_INFINITY;
+				m_hi[i] = BT_INFINITY;
+			}
+			else
+			{
+				m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
+				m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
+			}
+		}
+	}
+
+	//
+	int m = m_allConstraintPtrArray.size();
+
+	int numBodies = m_tmpSolverBodyPool.size();
+	btAlignedObjectArray<int> bodyJointNodeArray;
+	{
+		BT_PROFILE("bodyJointNodeArray.resize");
+		bodyJointNodeArray.resize(numBodies, -1);
+	}
+	btAlignedObjectArray<btJointNode1> jointNodeArray;
+	{
+		BT_PROFILE("jointNodeArray.reserve");
+		jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
+	}
+
+	btMatrixXu& J3 = m_scratchJ3;
+	{
+		BT_PROFILE("J3.resize");
+		J3.resize(2 * m, 8);
+	}
+	btMatrixXu& JinvM3 = m_scratchJInvM3;
+	{
+		BT_PROFILE("JinvM3.resize/setZero");
+
+		JinvM3.resize(2 * m, 8);
+		JinvM3.setZero();
+		J3.setZero();
+	}
+	int cur = 0;
+	int rowOffset = 0;
+	btAlignedObjectArray<int>& ofs = m_scratchOfs;
+	{
+		BT_PROFILE("ofs resize");
+		ofs.resize(0);
+		ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
+	}
+	{
+		BT_PROFILE("Compute J and JinvM");
+		int c = 0;
+
+		int numRows = 0;
+
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
+		{
+			ofs[c] = rowOffset;
+			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
+			btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+			btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+			numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
+			if (orgBodyA)
+			{
+				{
+					int slotA = -1;
+					//find free jointNode slot for sbA
+					slotA = jointNodeArray.size();
+					jointNodeArray.expand();  //NonInitializing();
+					int prevSlot = bodyJointNodeArray[sbA];
+					bodyJointNodeArray[sbA] = slotA;
+					jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
+					jointNodeArray[slotA].jointIndex = c;
+					jointNodeArray[slotA].constraintRowIndex = i;
+					jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
+				}
+				for (int row = 0; row < numRows; row++, cur++)
+				{
+					btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass();
+					btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
+
+					for (int r = 0; r < 3; r++)
+					{
+						J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
+						J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
+						JinvM3.setElem(cur, r, normalInvMass[r]);
+						JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
+					}
+					J3.setElem(cur, 3, 0);
+					JinvM3.setElem(cur, 3, 0);
+					J3.setElem(cur, 7, 0);
+					JinvM3.setElem(cur, 7, 0);
+				}
+			}
+			else
+			{
+				cur += numRows;
+			}
+			if (orgBodyB)
+			{
+				{
+					int slotB = -1;
+					//find free jointNode slot for sbA
+					slotB = jointNodeArray.size();
+					jointNodeArray.expand();  //NonInitializing();
+					int prevSlot = bodyJointNodeArray[sbB];
+					bodyJointNodeArray[sbB] = slotB;
+					jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
+					jointNodeArray[slotB].jointIndex = c;
+					jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
+					jointNodeArray[slotB].constraintRowIndex = i;
+				}
+
+				for (int row = 0; row < numRows; row++, cur++)
+				{
+					btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass();
+					btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
+
+					for (int r = 0; r < 3; r++)
+					{
+						J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
+						J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
+						JinvM3.setElem(cur, r, normalInvMassB[r]);
+						JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
+					}
+					J3.setElem(cur, 3, 0);
+					JinvM3.setElem(cur, 3, 0);
+					J3.setElem(cur, 7, 0);
+					JinvM3.setElem(cur, 7, 0);
+				}
+			}
+			else
+			{
+				cur += numRows;
+			}
+			rowOffset += numRows;
+		}
+	}
+
+	//compute JinvM = J*invM.
+	const btScalar* JinvM = JinvM3.getBufferPointer();
+
+	const btScalar* Jptr = J3.getBufferPointer();
+	{
+		BT_PROFILE("m_A.resize");
+		m_A.resize(n, n);
+	}
+
+	{
+		BT_PROFILE("m_A.setZero");
+		m_A.setZero();
+	}
+	int c = 0;
+	{
+		int numRows = 0;
+		BT_PROFILE("Compute A");
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
+		{
+			int row__ = ofs[c];
+			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
+			//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+			//	btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+			numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
+
+			const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
+
+			{
+				int startJointNodeA = bodyJointNodeArray[sbA];
+				while (startJointNodeA >= 0)
+				{
+					int j0 = jointNodeArray[startJointNodeA].jointIndex;
+					int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
+					if (j0 < c)
+					{
+						int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
+						size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0;
+						//printf("%d joint i %d and j0: %d: ",count++,i,j0);
+						m_A.multiplyAdd2_p8r(JinvMrow,
+											 Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
+					}
+					startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
+				}
+			}
+
+			{
+				int startJointNodeB = bodyJointNodeArray[sbB];
+				while (startJointNodeB >= 0)
+				{
+					int j1 = jointNodeArray[startJointNodeB].jointIndex;
+					int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
+
+					if (j1 < c)
+					{
+						int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
+						size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
+						m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows,
+											 Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
+					}
+					startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
+				}
+			}
+		}
+
+		{
+			BT_PROFILE("compute diagonal");
+			// compute diagonal blocks of m_A
+
+			int row__ = 0;
+			int numJointRows = m_allConstraintPtrArray.size();
+
+			int jj = 0;
+			for (; row__ < numJointRows;)
+			{
+				//int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
+				int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
+				//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+				btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+				const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
+
+				const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
+				const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
+				m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
+				if (orgBodyB)
+				{
+					m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
+				}
+				row__ += infom;
+				jj++;
+			}
+		}
+	}
+
+	if (1)
+	{
+		// add cfm to the diagonal of m_A
+		for (int i = 0; i < m_A.rows(); ++i)
+		{
+			m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
+		}
+	}
+
+	///fill the upper triangle of the matrix, to make it symmetric
+	{
+		BT_PROFILE("fill the upper triangle ");
+		m_A.copyLowerToUpperTriangle();
+	}
+
+	{
+		BT_PROFILE("resize/init x");
+		m_x.resize(numConstraintRows);
+		m_xSplit.resize(numConstraintRows);
+
+		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+		{
+			for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
+			{
+				const btSolverConstraint& c = *m_allConstraintPtrArray[i];
+				m_x[i] = c.m_appliedImpulse;
+				m_xSplit[i] = c.m_appliedPushImpulse;
+			}
+		}
+		else
+		{
+			m_x.setZero();
+			m_xSplit.setZero();
+		}
+	}
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal)
+{
+	const int multiBodyNumConstraints = m_multiBodyAllConstraintPtrArray.size();
+
+	if (multiBodyNumConstraints == 0)
+		return;
+
+	// 1. Compute b
+	{
+		BT_PROFILE("init b (rhs)");
+
+		m_multiBodyB.resize(multiBodyNumConstraints);
+		m_multiBodyB.setZero();
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			const btScalar jacDiag = constraint.m_jacDiagABInv;
+
+			if (!btFuzzyZero(jacDiag))
+			{
+				// Note that rhsPenetration is currently always zero because the split impulse hasn't been implemented for multibody yet.
+				const btScalar rhs = constraint.m_rhs;
+				m_multiBodyB[i] = rhs / jacDiag;
+			}
+		}
+	}
+
+	// 2. Compute lo and hi
+	{
+		BT_PROFILE("init lo/ho");
+
+		m_multiBodyLo.resize(multiBodyNumConstraints);
+		m_multiBodyHi.resize(multiBodyNumConstraints);
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			m_multiBodyLo[i] = constraint.m_lowerLimit;
+			m_multiBodyHi[i] = constraint.m_upperLimit;
+		}
+	}
+
+	// 3. Construct A matrix by using the impulse testing
+	{
+		BT_PROFILE("Compute A");
+
+		{
+			BT_PROFILE("m_A.resize");
+			m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints);
+		}
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			// Compute the diagonal of A, which is A(i, i)
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			const btScalar diagA = computeConstraintMatrixDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint);
+			m_multiBodyA.setElem(i, i, diagA);
+
+			// Computes the off-diagonals of A:
+			//   a. The rest of i-th row of A, from A(i, i+1) to A(i, n)
+			//   b. The rest of i-th column of A, from A(i+1, i) to A(n, i)
+			for (int j = i + 1; j < multiBodyNumConstraints; ++j)
+			{
+				const btMultiBodySolverConstraint& offDiagConstraint = *m_multiBodyAllConstraintPtrArray[j];
+				const btScalar offDiagA = computeConstraintMatrixOffDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint, offDiagConstraint);
+
+				// Set the off-diagonal values of A. Note that A is symmetric.
+				m_multiBodyA.setElem(i, j, offDiagA);
+				m_multiBodyA.setElem(j, i, offDiagA);
+			}
+		}
+	}
+
+	// Add CFM to the diagonal of m_A
+	for (int i = 0; i < m_multiBodyA.rows(); ++i)
+	{
+		m_multiBodyA.setElem(i, i, m_multiBodyA(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
+	}
+
+	// 4. Initialize x
+	{
+		BT_PROFILE("resize/init x");
+
+		m_multiBodyX.resize(multiBodyNumConstraints);
+
+		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+		{
+			for (int i = 0; i < multiBodyNumConstraints; ++i)
+			{
+				const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+				m_multiBodyX[i] = constraint.m_appliedImpulse;
+			}
+		}
+		else
+		{
+			m_multiBodyX.setZero();
+		}
+	}
+}
+
+bool btMultiBodyMLCPConstraintSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
+{
+	bool result = true;
+
+	if (m_A.rows() != 0)
+	{
+		// If using split impulse, we solve 2 separate (M)LCPs
+		if (infoGlobal.m_splitImpulse)
+		{
+			const btMatrixXu Acopy = m_A;
+			const btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
+			// TODO(JS): Do we really need these copies when solveMLCP takes them as const?
+
+			result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
+			if (result)
+				result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations);
+		}
+		else
+		{
+			result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
+		}
+	}
+
+	if (!result)
+		return false;
+
+	if (m_multiBodyA.rows() != 0)
+	{
+		result = m_solver->solveMLCP(m_multiBodyA, m_multiBodyB, m_multiBodyX, m_multiBodyLo, m_multiBodyHi, m_multiBodyLimitDependencies, infoGlobal.m_numIterations);
+	}
+
+	return result;
+}
+
+btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup(
+	btCollisionObject** bodies,
+	int numBodies,
+	btPersistentManifold** manifoldPtr,
+	int numManifolds,
+	btTypedConstraint** constraints,
+	int numConstraints,
+	const btContactSolverInfo& infoGlobal,
+	btIDebugDraw* debugDrawer)
+{
+	// 1. Setup for rigid-bodies
+	btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(
+		bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+
+	// 2. Setup for multi-bodies
+	//   a. Collect all different kinds of constraint as pointers into one array, m_allConstraintPtrArray
+	//   b. Set the index array for frictional contact constraints, m_limitDependencies
+	{
+		BT_PROFILE("gather constraint data");
+
+		int dindex = 0;
+
+		const int numRigidBodyConstraints = m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size();
+		const int numMultiBodyConstraints = m_multiBodyNonContactConstraints.size() + m_multiBodyNormalContactConstraints.size() + m_multiBodyFrictionContactConstraints.size();
+
+		m_allConstraintPtrArray.resize(0);
+		m_multiBodyAllConstraintPtrArray.resize(0);
+
+		// i. Setup for rigid bodies
+
+		m_limitDependencies.resize(numRigidBodyConstraints);
+
+		for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); ++i)
+		{
+			m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
+			m_limitDependencies[dindex++] = -1;
+		}
+
+		int firstContactConstraintOffset = dindex;
+
+		// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
+		if (interleaveContactAndFriction1)
+		{
+			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
+			{
+				const int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
+
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]);
+				int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
+				m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
+				if (numFrictionPerContact == 2)
+				{
+					m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
+					m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
+				}
+			}
+		}
+		else
+		{
+			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
+			{
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+			}
+			for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
+			{
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
+				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
+			}
+		}
+
+		if (!m_allConstraintPtrArray.size())
+		{
+			m_A.resize(0, 0);
+			m_b.resize(0);
+			m_x.resize(0);
+			m_lo.resize(0);
+			m_hi.resize(0);
+		}
+
+		// ii. Setup for multibodies
+
+		dindex = 0;
+
+		m_multiBodyLimitDependencies.resize(numMultiBodyConstraints);
+
+		for (int i = 0; i < m_multiBodyNonContactConstraints.size(); ++i)
+		{
+			m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNonContactConstraints[i]);
+			m_multiBodyLimitDependencies[dindex++] = -1;
+		}
+
+		firstContactConstraintOffset = dindex;
+
+		// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
+		if (interleaveContactAndFriction1)
+		{
+			for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
+			{
+				const int numtiBodyNumFrictionPerContact = m_multiBodyNormalContactConstraints.size() == m_multiBodyFrictionContactConstraints.size() ? 1 : 2;
+
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = -1;
+
+				btMultiBodySolverConstraint& frictionContactConstraint1 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact];
+				m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint1);
+
+				const int findex = (frictionContactConstraint1.m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset;
+
+				m_multiBodyLimitDependencies[dindex++] = findex;
+
+				if (numtiBodyNumFrictionPerContact == 2)
+				{
+					btMultiBodySolverConstraint& frictionContactConstraint2 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact + 1];
+					m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint2);
+
+					m_multiBodyLimitDependencies[dindex++] = findex;
+				}
+			}
+		}
+		else
+		{
+			for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
+			{
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = -1;
+			}
+			for (int i = 0; i < m_multiBodyFrictionContactConstraints.size(); ++i)
+			{
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyFrictionContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = m_multiBodyFrictionContactConstraints[i].m_frictionIndex + firstContactConstraintOffset;
+			}
+		}
+
+		if (!m_multiBodyAllConstraintPtrArray.size())
+		{
+			m_multiBodyA.resize(0, 0);
+			m_multiBodyB.resize(0);
+			m_multiBodyX.resize(0);
+			m_multiBodyLo.resize(0);
+			m_multiBodyHi.resize(0);
+		}
+	}
+
+	// Construct MLCP terms
+	{
+		BT_PROFILE("createMLCPFast");
+		createMLCPFast(infoGlobal);
+	}
+
+	return btScalar(0);
+}
+
+btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+	bool result = true;
+	{
+		BT_PROFILE("solveMLCP");
+		result = solveMLCP(infoGlobal);
+	}
+
+	// Fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations if the solution isn't valid.
+	if (!result)
+	{
+		m_fallback++;
+		return btMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+	}
+
+	{
+		BT_PROFILE("process MLCP results");
+
+		for (int i = 0; i < m_allConstraintPtrArray.size(); ++i)
+		{
+			const btSolverConstraint& c = *m_allConstraintPtrArray[i];
+
+			const btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse;
+			c.m_appliedImpulse = m_x[i];
+
+			int sbA = c.m_solverBodyIdA;
+			int sbB = c.m_solverBodyIdB;
+
+			btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
+			btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
+
+			solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+			solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+
+			if (infoGlobal.m_splitImpulse)
+			{
+				const btScalar deltaPushImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
+				solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaPushImpulse);
+				solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaPushImpulse);
+				c.m_appliedPushImpulse = m_xSplit[i];
+			}
+		}
+
+		for (int i = 0; i < m_multiBodyAllConstraintPtrArray.size(); ++i)
+		{
+			btMultiBodySolverConstraint& c = *m_multiBodyAllConstraintPtrArray[i];
+
+			const btScalar deltaImpulse = m_multiBodyX[i] - c.m_appliedImpulse;
+			c.m_appliedImpulse = m_multiBodyX[i];
+
+			btMultiBody* multiBodyA = c.m_multiBodyA;
+			if (multiBodyA)
+			{
+				const int ndofA = multiBodyA->getNumDofs() + 6;
+				applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+				//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+				//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+				multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
+#endif  // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+			}
+			else
+			{
+				const int sbA = c.m_solverBodyIdA;
+				btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
+				solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+			}
+
+			btMultiBody* multiBodyB = c.m_multiBodyB;
+			if (multiBodyB)
+			{
+				const int ndofB = multiBodyB->getNumDofs() + 6;
+				applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+				//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+				//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+				multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
+#endif  // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+			}
+			else
+			{
+				const int sbB = c.m_solverBodyIdB;
+				btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
+				solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+			}
+		}
+	}
+
+	return btScalar(0);
+}
+
+btMultiBodyMLCPConstraintSolver::btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver)
+	: m_solver(solver), m_fallback(0)
+{
+	// Do nothing
+}
+
+btMultiBodyMLCPConstraintSolver::~btMultiBodyMLCPConstraintSolver()
+{
+	// Do nothing
+}
+
+void btMultiBodyMLCPConstraintSolver::setMLCPSolver(btMLCPSolverInterface* solver)
+{
+	m_solver = solver;
+}
+
+int btMultiBodyMLCPConstraintSolver::getNumFallbacks() const
+{
+	return m_fallback;
+}
+
+void btMultiBodyMLCPConstraintSolver::setNumFallbacks(int num)
+{
+	m_fallback = num;
+}
+
+btConstraintSolverType btMultiBodyMLCPConstraintSolver::getSolverType() const
+{
+	return BT_MLCP_SOLVER;
+}

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h

@@ -0,0 +1,187 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
+#define BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
+
+#include "LinearMath/btMatrixX.h"
+#include "LinearMath/btThreads.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+
+class btMLCPSolverInterface;
+class btMultiBody;
+
+class btMultiBodyMLCPConstraintSolver : public btMultiBodyConstraintSolver
+{
+protected:
+	/// \name MLCP Formulation for Rigid Bodies
+	/// \{
+
+	/// A matrix in the MLCP formulation
+	btMatrixXu m_A;
+
+	/// b vector in the MLCP formulation.
+	btVectorXu m_b;
+
+	/// Constraint impulse, which is an output of MLCP solving.
+	btVectorXu m_x;
+
+	/// Lower bound of constraint impulse, \c m_x.
+	btVectorXu m_lo;
+
+	/// Upper bound of constraint impulse, \c m_x.
+	btVectorXu m_hi;
+
+	/// \}
+
+	/// \name Cache Variables for Split Impulse for Rigid Bodies
+	/// When using 'split impulse' we solve two separate (M)LCPs
+	/// \{
+
+	/// Split impulse Cache vector corresponding to \c m_b.
+	btVectorXu m_bSplit;
+
+	/// Split impulse cache vector corresponding to \c m_x.
+	btVectorXu m_xSplit;
+
+	/// \}
+
+	/// \name MLCP Formulation for Multibodies
+	/// \{
+
+	/// A matrix in the MLCP formulation
+	btMatrixXu m_multiBodyA;
+
+	/// b vector in the MLCP formulation.
+	btVectorXu m_multiBodyB;
+
+	/// Constraint impulse, which is an output of MLCP solving.
+	btVectorXu m_multiBodyX;
+
+	/// Lower bound of constraint impulse, \c m_x.
+	btVectorXu m_multiBodyLo;
+
+	/// Upper bound of constraint impulse, \c m_x.
+	btVectorXu m_multiBodyHi;
+
+	/// \}
+
+	/// Indices of normal contact constraint associated with frictional contact constraint for rigid bodies.
+	///
+	/// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate
+	/// normal contact impulse. For example, i-th element represents the index of a normal constraint that is
+	/// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint.
+	/// Otherwise, -1.
+	btAlignedObjectArray<int> m_limitDependencies;
+
+	/// Indices of normal contact constraint associated with frictional contact constraint for multibodies.
+	///
+	/// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate
+	/// normal contact impulse. For example, i-th element represents the index of a normal constraint that is
+	/// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint.
+	/// Otherwise, -1.
+	btAlignedObjectArray<int> m_multiBodyLimitDependencies;
+
+	/// Array of all the rigid body constraints
+	btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray;
+
+	/// Array of all the multibody constraints
+	btAlignedObjectArray<btMultiBodySolverConstraint*> m_multiBodyAllConstraintPtrArray;
+
+	/// MLCP solver
+	btMLCPSolverInterface* m_solver;
+
+	/// Count of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP solver fails.
+	int m_fallback;
+
+	/// \name MLCP Scratch Variables
+	/// The following scratch variables are not stateful -- contents are cleared prior to each use.
+	/// They are only cached here to avoid extra memory allocations and deallocations and to ensure
+	/// that multiple instances of the solver can be run in parallel.
+	///
+	/// \{
+
+	/// Cache variable for constraint Jacobian matrix.
+	btMatrixXu m_scratchJ3;
+
+	/// Cache variable for constraint Jacobian times inverse mass matrix.
+	btMatrixXu m_scratchJInvM3;
+
+	/// Cache variable for offsets.
+	btAlignedObjectArray<int> m_scratchOfs;
+
+	/// \}
+
+	/// Constructs MLCP terms, which are \c m_A, \c m_b, \c m_lo, and \c m_hi.
+	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
+
+	/// Constructs MLCP terms for constraints of two rigid bodies
+	void createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal);
+
+	/// Constructs MLCP terms for constraints of two multi-bodies or one rigid body and one multibody
+	void createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal);
+
+	/// Solves MLCP and returns the success
+	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
+
+	// Documentation inherited
+	btScalar solveGroupCacheFriendlySetup(
+		btCollisionObject** bodies,
+		int numBodies,
+		btPersistentManifold** manifoldPtr,
+		int numManifolds,
+		btTypedConstraint** constraints,
+		int numConstraints,
+		const btContactSolverInfo& infoGlobal,
+		btIDebugDraw* debugDrawer) BT_OVERRIDE;
+
+	// Documentation inherited
+	btScalar solveGroupCacheFriendlyIterations(
+		btCollisionObject** bodies,
+		int numBodies,
+		btPersistentManifold** manifoldPtr,
+		int numManifolds,
+		btTypedConstraint** constraints,
+		int numConstraints,
+		const btContactSolverInfo& infoGlobal,
+		btIDebugDraw* debugDrawer) ;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR()
+
+	/// Constructor
+	///
+	/// \param[in] solver MLCP solver. Assumed it's not null.
+	/// \param[in] maxLCPSize Maximum size of LCP to solve using MLCP solver. If the MLCP size exceeds this number, sequaltial impulse method will be used.
+	explicit btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver);
+
+	/// Destructor
+	virtual ~btMultiBodyMLCPConstraintSolver();
+
+	/// Sets MLCP solver. Assumed it's not null.
+	void setMLCPSolver(btMLCPSolverInterface* solver);
+
+	/// Returns the number of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP
+	/// solver fails.
+	int getNumFallbacks() const;
+
+	/// Sets the number of fallbacks. This function may be used to reset the number to zero.
+	void setNumFallbacks(int num);
+
+	/// Returns the constraint solver type.
+	virtual btConstraintSolverType getSolverType() const;
+};
+
+#endif  // BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp

@@ -0,0 +1,216 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyPoint2Point.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btIDebugDraw.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, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(bodyB),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB)
+{
+	m_data.resize(BTMBP2PCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(0),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB)
+{
+	m_data.resize(BTMBP2PCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+void btMultiBodyPoint2Point::finalizeMultiDof()
+{
+	//not implemented yet
+	btAssert(0);
+}
+
+btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
+{
+}
+
+int btMultiBodyPoint2Point::getIslandIdA() const
+{
+	if (m_rigidBodyA)
+		return m_rigidBodyA->getIslandTag();
+
+	if (m_bodyA)
+	{
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodyPoint2Point::getIslandIdB() const
+{
+	if (m_rigidBodyB)
+		return m_rigidBodyB->getIslandTag();
+	if (m_bodyB)
+	{
+		if (m_linkB < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+												  btMultiBodyJacobianData& data,
+												  const btContactSolverInfo& infoGlobal)
+{
+	//	int i=1;
+	int numDim = BTMBP2PCONSTRAINT_DIM;
+	for (int i = 0; i < numDim; i++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		//memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = i;
+		constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal1.setValue(0, 0, 0);
+		constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal2.setValue(0, 0, 0);
+		constraintRow.m_angularComponentA.setValue(0, 0, 0);
+		constraintRow.m_angularComponentB.setValue(0, 0, 0);
+
+		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
+
+		// Convert local points back to world
+		btVector3 pivotAworld = m_pivotInA;
+		if (m_rigidBodyA)
+		{
+			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
+			pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		}
+		else
+		{
+			if (m_bodyA)
+				pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		}
+		btVector3 pivotBworld = m_pivotInB;
+		if (m_rigidBodyB)
+		{
+			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
+			pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		}
+		else
+		{
+			if (m_bodyB)
+				pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		}
+
+		btScalar posError = i < 3 ? (pivotAworld - pivotBworld).dot(contactNormalOnB) : 0;
+
+#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
+
+		fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0, 0, 0),
+								contactNormalOnB, pivotAworld, pivotBworld,  //sucks but let it be this way "for the time being"
+								posError,
+								infoGlobal,
+								-m_maxAppliedImpulse, m_maxAppliedImpulse);
+		//@todo: support the case of btMultiBody versus btRigidBody,
+		//see btPoint2PointConstraint::getInfo2NonVirtual
+#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
+	}
+}
+
+void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer)
+{
+	btTransform tr;
+	tr.setIdentity();
+
+	if (m_rigidBodyA)
+	{
+		btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyA)
+	{
+		btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		tr.setOrigin(pivotAworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_rigidBodyB)
+	{
+		// that ideally should draw the same frame
+		btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyB)
+	{
+		btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		tr.setOrigin(pivotBworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+}

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h

@@ -0,0 +1,64 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_POINT2POINT_H
+#define BT_MULTIBODY_POINT2POINT_H
+
+#include "btMultiBodyConstraint.h"
+
+//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
+
+ATTRIBUTE_ALIGNED16(class)
+btMultiBodyPoint2Point : public btMultiBodyConstraint
+{
+protected:
+	btRigidBody* m_rigidBodyA;
+	btRigidBody* m_rigidBodyB;
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btMultiBodyPoint2Point(btMultiBody * body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
+	btMultiBodyPoint2Point(btMultiBody * bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);
+
+	virtual ~btMultiBodyPoint2Point();
+
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows,
+									  btMultiBodyJacobianData & data,
+									  const btContactSolverInfo& infoGlobal);
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	virtual void setPivotInB(const btVector3& pivotInB)
+	{
+		m_pivotInB = pivotInB;
+	}
+
+	virtual void debugDraw(class btIDebugDraw * drawer);
+};
+
+#endif  //BT_MULTIBODY_POINT2POINT_H

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp

@@ -0,0 +1,234 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodySliderConstraint.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#define BTMBSLIDERCONSTRAINT_DIM 5
+#define EPSILON 0.000001
+
+btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
+	: btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(bodyB),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB),
+	  m_jointAxis(jointAxis)
+{
+	m_data.resize(BTMBSLIDERCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(0),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB),
+	  m_jointAxis(jointAxis)
+{
+	m_data.resize(BTMBSLIDERCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+void btMultiBodySliderConstraint::finalizeMultiDof()
+{
+	//not implemented yet
+	btAssert(0);
+}
+
+btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
+{
+}
+
+int btMultiBodySliderConstraint::getIslandIdA() const
+{
+	if (m_rigidBodyA)
+		return m_rigidBodyA->getIslandTag();
+
+	if (m_bodyA)
+	{
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodySliderConstraint::getIslandIdB() const
+{
+	if (m_rigidBodyB)
+		return m_rigidBodyB->getIslandTag();
+	if (m_bodyB)
+	{
+		if (m_linkB < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
+{
+	// Convert local points back to world
+	btVector3 pivotAworld = m_pivotInA;
+	btMatrix3x3 frameAworld = m_frameInA;
+	btVector3 jointAxis = m_jointAxis;
+	if (m_rigidBodyA)
+	{
+		pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
+		jointAxis = quatRotate(m_rigidBodyA->getOrientation(), m_jointAxis);
+	}
+	else if (m_bodyA)
+	{
+		pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
+		jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
+	}
+	btVector3 pivotBworld = m_pivotInB;
+	btMatrix3x3 frameBworld = m_frameInB;
+	if (m_rigidBodyB)
+	{
+		pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		frameBworld = m_frameInB.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
+	}
+	else if (m_bodyB)
+	{
+		pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
+	}
+
+	btVector3 constraintAxis[2];
+	for (int i = 0; i < 3; ++i)
+	{
+		constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
+		if (constraintAxis[0].safeNorm() > EPSILON)
+		{
+			constraintAxis[0] = constraintAxis[0].normalized();
+			constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
+			constraintAxis[1] = constraintAxis[1].normalized();
+			break;
+		}
+	}
+
+	btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
+	btVector3 angleDiff;
+	btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
+
+	int numDim = BTMBSLIDERCONSTRAINT_DIM;
+	for (int i = 0; i < numDim; i++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = i;
+		constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal1.setValue(0, 0, 0);
+		constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal2.setValue(0, 0, 0);
+		constraintRow.m_angularComponentA.setValue(0, 0, 0);
+		constraintRow.m_angularComponentB.setValue(0, 0, 0);
+
+		constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
+		constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
+
+		if (m_rigidBodyA)
+		{
+			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
+		}
+		if (m_rigidBodyB)
+		{
+			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
+		}
+
+		btVector3 constraintNormalLin(0, 0, 0);
+		btVector3 constraintNormalAng(0, 0, 0);
+		btScalar posError = 0.0;
+		if (i < 2)
+		{
+			constraintNormalLin = constraintAxis[i];
+			posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse);
+		}
+		else
+		{  //i>=2
+			constraintNormalAng = frameAworld.getColumn(i % 3);
+			posError = angleDiff[i % 3];
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
+		}
+	}
+}
+
+void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
+{
+	btTransform tr;
+	tr.setIdentity();
+
+	if (m_rigidBodyA)
+	{
+		btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyA)
+	{
+		btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		tr.setOrigin(pivotAworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_rigidBodyB)
+	{
+		// that ideally should draw the same frame
+		btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyB)
+	{
+		btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		tr.setOrigin(pivotBworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+}

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h

@@ -0,0 +1,102 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_SLIDER_CONSTRAINT_H
+#define BT_MULTIBODY_SLIDER_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+
+class btMultiBodySliderConstraint : public btMultiBodyConstraint
+{
+protected:
+	btRigidBody* m_rigidBodyA;
+	btRigidBody* m_rigidBodyB;
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
+	btMatrix3x3 m_frameInA;
+	btMatrix3x3 m_frameInB;
+	btVector3 m_jointAxis;
+
+public:
+	btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
+	btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
+
+	virtual ~btMultiBodySliderConstraint();
+
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	const btVector3& getPivotInA() const
+	{
+		return m_pivotInA;
+	}
+
+	void setPivotInA(const btVector3& pivotInA)
+	{
+		m_pivotInA = pivotInA;
+	}
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	virtual void setPivotInB(const btVector3& pivotInB)
+	{
+		m_pivotInB = pivotInB;
+	}
+
+	const btMatrix3x3& getFrameInA() const
+	{
+		return m_frameInA;
+	}
+
+	void setFrameInA(const btMatrix3x3& frameInA)
+	{
+		m_frameInA = frameInA;
+	}
+
+	const btMatrix3x3& getFrameInB() const
+	{
+		return m_frameInB;
+	}
+
+	virtual void setFrameInB(const btMatrix3x3& frameInB)
+	{
+		m_frameInB = frameInB;
+	}
+
+	const btVector3& getJointAxis() const
+	{
+		return m_jointAxis;
+	}
+
+	void setJointAxis(const btVector3& jointAxis)
+	{
+		m_jointAxis = jointAxis;
+	}
+
+	virtual void debugDraw(class btIDebugDraw* drawer);
+};
+
+#endif  //BT_MULTIBODY_SLIDER_CONSTRAINT_H

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h

@@ -0,0 +1,90 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H
+#define BT_MULTIBODY_SOLVER_CONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+class btMultiBody;
+class btMultiBodyConstraint;
+#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+
+///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
+ATTRIBUTE_ALIGNED16(struct)
+btMultiBodySolverConstraint
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1), m_orgConstraint(0), m_orgDofIndex(-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;
+	btVector3 m_relpos2CrossNormal;
+	btVector3 m_contactNormal2;  //usually m_contactNormal2 == -m_contactNormal1, but not always
+
+	btVector3 m_angularComponentA;
+	btVector3 m_angularComponentB;
+
+	mutable btSimdScalar m_appliedPushImpulse;
+	mutable btSimdScalar m_appliedImpulse;
+
+	btScalar m_friction;
+	btScalar m_jacDiagABInv;
+	btScalar m_rhs;
+	btScalar m_cfm;
+
+	btScalar m_lowerLimit;
+	btScalar m_upperLimit;
+	btScalar m_rhsPenetration;
+	union {
+		void* m_originalContactPoint;
+		btScalar m_unusedPadding4;
+	};
+
+	int m_overrideNumSolverIterations;
+	int m_frictionIndex;
+
+	int m_solverBodyIdA;
+	btMultiBody* m_multiBodyA;
+	int m_linkA;
+
+	int m_solverBodyIdB;
+	btMultiBody* m_multiBodyB;
+	int m_linkB;
+
+	//for writing back applied impulses
+	btMultiBodyConstraint* m_orgConstraint;
+	int m_orgDofIndex;
+
+	enum btSolverConstraintType
+	{
+		BT_SOLVER_CONTACT_1D = 0,
+		BT_SOLVER_FRICTION_1D
+	};
+};
+
+typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray;
+
+#endif  //BT_MULTIBODY_SOLVER_CONSTRAINT_H

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp

@@ -0,0 +1,180 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodySphericalJointMotor.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "LinearMath/btTransformUtil.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+
+btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR),
+	m_desiredVelocity(0, 0, 0),
+	m_desiredPosition(0,0,0,1),
+	m_use_multi_dof_params(false),
+	m_kd(1., 1., 1.),
+	m_kp(0.2, 0.2, 0.2),
+	m_erp(1),
+	m_rhsClamp(SIMD_INFINITY),
+	m_maxAppliedImpulseMultiDof(maxMotorImpulse, maxMotorImpulse, maxMotorImpulse),
+	m_damping(1.0, 1.0, 1.0)
+{
+
+	m_maxAppliedImpulse = maxMotorImpulse;
+}
+
+
+void btMultiBodySphericalJointMotor::finalizeMultiDof()
+{
+	allocateJacobiansMultiDof();
+	// note: we rely on the fact that data.m_jacobians are
+	// always initialized to zero by the Constraint ctor
+	int linkDoF = 0;
+	unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
+
+	// row 0: the lower bound
+	// row 0: the lower bound
+	jacobianA(0)[offset] = 1;
+
+	m_numDofsFinalized = m_jacSizeBoth;
+}
+
+
+btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor()
+{
+}
+
+int btMultiBodySphericalJointMotor::getIslandIdA() const
+{
+	if (this->m_linkA < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyA->getLink(m_linkA).m_collider)
+		{
+			return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodySphericalJointMotor::getIslandIdB() const
+{
+	if (m_linkB < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyB->getLink(m_linkB).m_collider)
+		{
+			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodySphericalJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+												 btMultiBodyJacobianData& data,
+												 const btContactSolverInfo& infoGlobal)
+{
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+		finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+	
+
+	if (m_maxAppliedImpulse == 0.f)
+		return;
+
+	const btScalar posError = 0;
+	const btVector3 dummy(0, 0, 0);
+
+	
+	btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
+	
+	btQuaternion desiredQuat = m_desiredPosition;
+	btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0],
+		m_bodyA->getJointPosMultiDof(m_linkA)[1],
+		m_bodyA->getJointPosMultiDof(m_linkA)[2],
+		m_bodyA->getJointPosMultiDof(m_linkA)[3]);
+
+btQuaternion relRot = currentQuat.inverse() * desiredQuat;
+	btVector3 angleDiff;
+	btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff);
+
+
+
+	for (int row = 0; row < getNumRows(); row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+
+		int dof = row;
+		
+		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar desiredVelocity = this->m_desiredVelocity[row];
+		
+		double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0];
+		btScalar velocityError = (desiredVelocity - currentVelocity) * kd;
+
+		btMatrix3x3 frameAworld;
+		frameAworld.setIdentity();
+		frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
+		btScalar posError = 0;
+		{
+			btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical);
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eSpherical:
+				{
+					btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
+					double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0];
+					posError = kp*angleDiff[row % 3];
+					double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse;
+					fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+						btVector3(0,0,0), dummy, dummy,
+						posError,
+						infoGlobal,
+						-max_applied_impulse, max_applied_impulse, true,
+						1.0, false, 0, 0,
+						m_damping[row % 3]);
+					constraintRow.m_orgConstraint = this;
+					constraintRow.m_orgDofIndex = row;
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+		}
+	}
+}

thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h

@@ -0,0 +1,118 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H
+#define BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H
+
+#include "btMultiBodyConstraint.h"
+struct btSolverInfo;
+
+class btMultiBodySphericalJointMotor : public btMultiBodyConstraint
+{
+protected:
+	btVector3 m_desiredVelocity;
+	btQuaternion m_desiredPosition;
+	bool m_use_multi_dof_params;
+	btVector3 m_kd;
+	btVector3 m_kp;
+	btScalar m_erp;
+	btScalar m_rhsClamp;  //maximum error
+	btVector3 m_maxAppliedImpulseMultiDof;
+	btVector3 m_damping;
+
+public:
+	btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse);
+	
+	virtual ~btMultiBodySphericalJointMotor();
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.0)
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = btVector3(kd, kd, kd);
+		m_use_multi_dof_params = false;
+	}
+
+	virtual void setVelocityTargetMultiDof(const btVector3& velTarget, const btVector3& kd = btVector3(1.0, 1.0, 1.0))
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = kd;
+		m_use_multi_dof_params = true;
+	}
+
+	virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp =1.f)
+	{
+		m_desiredPosition = posTarget;
+		m_kp = btVector3(kp, kp, kp);
+		m_use_multi_dof_params = false;
+	}
+
+	virtual void setPositionTargetMultiDof(const btQuaternion& posTarget, const btVector3& kp = btVector3(1.f, 1.f, 1.f))
+	{
+		m_desiredPosition = posTarget;
+		m_kp = kp;
+		m_use_multi_dof_params = true;
+	}
+
+	virtual void setErp(btScalar erp)
+	{
+		m_erp = erp;
+	}
+	virtual btScalar getErp() const
+	{
+		return m_erp;
+	}
+	virtual void setRhsClamp(btScalar rhsClamp)
+	{
+		m_rhsClamp = rhsClamp;
+	}
+
+	btScalar getMaxAppliedImpulseMultiDof(int i) const
+	{
+		return m_maxAppliedImpulseMultiDof[i];
+	}
+
+	void setMaxAppliedImpulseMultiDof(const btVector3& maxImp)
+	{
+		m_maxAppliedImpulseMultiDof = maxImp;
+		m_use_multi_dof_params = true;
+	}
+
+	btScalar getDamping(int i) const
+	{
+		return m_damping[i];
+	}
+
+	void setDamping(const btVector3& damping)
+	{
+		m_damping = damping;
+	}
+
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
+};
+
+#endif  //BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H