bullet: Streamline bundling, remove extraneous src/ folder

Document version and how to extract sources in thirdparty/README.md.
Drop unnecessary CMake and Premake files.
Simplify SCsub, drop unused one.

24 files changed, 0 insertions, 8341 deletions

diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp
deleted file mode 100644
index 62865e0c78..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ /dev/null
@@ -1,2043 +0,0 @@
-/*
- * 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.
- 
- */
-
-
-#include "btMultiBody.h"
-#include "btMultiBodyLink.h"
-#include "btMultiBodyLinkCollider.h"
-#include "btMultiBodyJointFeedback.h"
-#include "LinearMath/btTransformUtil.h"
-#include "LinearMath/btSerializer.h"
-//#include "Bullet3Common/b3Logging.h"
-// #define INCLUDE_GYRO_TERM 
-
-///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals
-bool gJointFeedbackInWorldSpace = false;
-bool gJointFeedbackInJointFrame = false;
-
-namespace {
-    const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
-    const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
-}
-
-namespace {
-    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
-    {
-        top_out = rotation_matrix * top_in;
-        bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
-    }
-
-#if 0
-    void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
-                                 const btVector3 &displacement,
-                                 const btVector3 &top_in,
-                                 const btVector3 &bottom_in,
-                                 btVector3 &top_out,
-                                 btVector3 &bottom_out)
-    {
-        top_out = rotation_matrix.transpose() * top_in;
-        bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));		
-    }
-
-    btScalar SpatialDotProduct(const btVector3 &a_top,
-                            const btVector3 &a_bottom,
-                            const btVector3 &b_top,
-                            const btVector3 &b_bottom)
-    {
-        return a_bottom.dot(b_top) + a_top.dot(b_bottom);
-    }
-
-	void SpatialCrossProduct(const btVector3 &a_top,
-                            const btVector3 &a_bottom,
-                            const btVector3 &b_top,
-                            const btVector3 &b_bottom,
-							btVector3 &top_out,
-							btVector3 &bottom_out)
-	{
-		top_out = a_top.cross(b_top);
-		bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
-	}
-#endif
-	
-}
-
-
-//
-// Implementation of class btMultiBody
-//
-
-btMultiBody::btMultiBody(int n_links,
-                     btScalar mass,
-                     const btVector3 &inertia,
-                     bool fixedBase,
-                     bool canSleep,
-		     bool /*deprecatedUseMultiDof*/)
-    : 
-    	m_baseCollider(0),
-		m_baseName(0),
-    	m_basePos(0,0,0),
-    	m_baseQuat(0, 0, 0, 1),
-      m_baseMass(mass),
-      m_baseInertia(inertia),
-    
-		m_fixedBase(fixedBase),
-		m_awake(true),
-		m_canSleep(canSleep),
-		m_sleepTimer(0),
-		m_userObjectPointer(0),
-		m_userIndex2(-1),
-		m_userIndex(-1),
-		m_linearDamping(0.04f),
-		m_angularDamping(0.04f),
-		m_useGyroTerm(true),
-			m_maxAppliedImpulse(1000.f),
-		m_maxCoordinateVelocity(100.f),
-			m_hasSelfCollision(true),		
-		__posUpdated(false),
-			m_dofCount(0),
-		m_posVarCnt(0),
-		m_useRK4(false), 	
-		m_useGlobalVelocities(false),
-		m_internalNeedsJointFeedback(false)
-{
-	m_cachedInertiaTopLeft.setValue(0,0,0,0,0,0,0,0,0);
-	m_cachedInertiaTopRight.setValue(0,0,0,0,0,0,0,0,0);
-	m_cachedInertiaLowerLeft.setValue(0,0,0,0,0,0,0,0,0);
-	m_cachedInertiaLowerRight.setValue(0,0,0,0,0,0,0,0,0);
-	m_cachedInertiaValid=false;
-
-	m_links.resize(n_links);
-	m_matrixBuf.resize(n_links + 1);
-
-    m_baseForce.setValue(0, 0, 0);
-    m_baseTorque.setValue(0, 0, 0);
-}
-
-btMultiBody::~btMultiBody()
-{
-}
-
-void btMultiBody::setupFixed(int i,
-						   btScalar mass,
-						   const btVector3 &inertia,
-						   int parent,
-						   const btQuaternion &rotParentToThis,
-						   const btVector3 &parentComToThisPivotOffset,
-                           const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/)
-{
-	
-	m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-	 m_links[i].setAxisTop(0, 0., 0., 0.);
-    m_links[i].setAxisBottom(0, btVector3(0,0,0));
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;
-	m_links[i].m_dVector = thisPivotToThisComOffset;
-    m_links[i].m_eVector = parentComToThisPivotOffset;    
-
-	m_links[i].m_jointType = btMultibodyLink::eFixed;
-	m_links[i].m_dofCount = 0;
-	m_links[i].m_posVarCount = 0;
-
-	m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;    
-	
-	m_links[i].updateCacheMultiDof();
-
-	updateLinksDofOffsets();
-
-}
-
-
-void btMultiBody::setupPrismatic(int i,
-                               btScalar mass,
-                               const btVector3 &inertia,
-                               int parent,
-                               const btQuaternion &rotParentToThis,
-                               const btVector3 &jointAxis,
-                               const btVector3 &parentComToThisPivotOffset,
-							   const btVector3 &thisPivotToThisComOffset,
-							   bool disableParentCollision)
-{
-	m_dofCount += 1;
-	m_posVarCnt += 1;
-	
-    m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;
-    m_links[i].setAxisTop(0, 0., 0., 0.);
-    m_links[i].setAxisBottom(0, jointAxis);
-    m_links[i].m_eVector = parentComToThisPivotOffset;
-	m_links[i].m_dVector = thisPivotToThisComOffset;
-    m_links[i].m_cachedRotParentToThis = rotParentToThis;
-
-	m_links[i].m_jointType = btMultibodyLink::ePrismatic;
-	m_links[i].m_dofCount = 1;
-	m_links[i].m_posVarCount = 1;	
-	m_links[i].m_jointPos[0] = 0.f;
-	m_links[i].m_jointTorque[0] = 0.f;
-
-	if (disableParentCollision)
-		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
-	//
-	
-	m_links[i].updateCacheMultiDof();
-	
-	updateLinksDofOffsets();
-}
-
-void btMultiBody::setupRevolute(int i,
-                              btScalar mass,
-                              const btVector3 &inertia,
-                              int parent,
-                              const btQuaternion &rotParentToThis,
-                              const btVector3 &jointAxis,
-                              const btVector3 &parentComToThisPivotOffset,
-                              const btVector3 &thisPivotToThisComOffset,
-							  bool disableParentCollision)
-{
-	m_dofCount += 1;
-	m_posVarCnt += 1;
-	
-    m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;
-    m_links[i].setAxisTop(0, jointAxis);
-    m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
-    m_links[i].m_dVector = thisPivotToThisComOffset;
-    m_links[i].m_eVector = parentComToThisPivotOffset;
-
-	m_links[i].m_jointType = btMultibodyLink::eRevolute;
-	m_links[i].m_dofCount = 1;
-	m_links[i].m_posVarCount = 1;	
-	m_links[i].m_jointPos[0] = 0.f;
-	m_links[i].m_jointTorque[0] = 0.f;
-
-	if (disableParentCollision)
-		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
-    //
-	m_links[i].updateCacheMultiDof();
-	//
-	updateLinksDofOffsets();
-}
-
-
-
-void btMultiBody::setupSpherical(int i,
-						   btScalar mass,
-						   const btVector3 &inertia,
-						   int parent,
-						   const btQuaternion &rotParentToThis,
-						   const btVector3 &parentComToThisPivotOffset,
-						   const btVector3 &thisPivotToThisComOffset,
-						   bool disableParentCollision)
-{
-	
-	m_dofCount += 3;
-	m_posVarCnt += 4;
-
-	m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;    
-    m_links[i].m_dVector = thisPivotToThisComOffset;
-    m_links[i].m_eVector = parentComToThisPivotOffset;    
-
-	m_links[i].m_jointType = btMultibodyLink::eSpherical;
-	m_links[i].m_dofCount = 3;
-	m_links[i].m_posVarCount = 4;
-	m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
-	m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
-	m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
-	m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
-	m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
-	m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
-	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointPos[3] = 1.f;
-	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
-
-
-	if (disableParentCollision)
-		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;    
-	//
-	m_links[i].updateCacheMultiDof();	
-	//
-	updateLinksDofOffsets();
-}
-
-void btMultiBody::setupPlanar(int i,
-						   btScalar mass,
-						   const btVector3 &inertia,
-						   int parent,
-						   const btQuaternion &rotParentToThis,
-						   const btVector3 &rotationAxis,
-						   const btVector3 &parentComToThisComOffset,						   
-						   bool disableParentCollision)
-{
-	
-	m_dofCount += 3;
-	m_posVarCnt += 3;
-
-	m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;    
-	m_links[i].m_dVector.setZero();
-    m_links[i].m_eVector = parentComToThisComOffset;
-
-	//
-	btVector3 vecNonParallelToRotAxis(1, 0, 0);
-	if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
-		vecNonParallelToRotAxis.setValue(0, 1, 0);
-	//
-
-	m_links[i].m_jointType = btMultibodyLink::ePlanar;
-	m_links[i].m_dofCount = 3;
-	m_links[i].m_posVarCount = 3;
-	btVector3 n=rotationAxis.normalized();
-	m_links[i].setAxisTop(0, n[0],n[1],n[2]);
-	m_links[i].setAxisTop(1,0,0,0);
-	m_links[i].setAxisTop(2,0,0,0);
-	m_links[i].setAxisBottom(0,0,0,0);
-	btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
-	m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
-	cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
-	m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
-	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
-	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
-
-	if (disableParentCollision)
-		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
-    //
-	m_links[i].updateCacheMultiDof();
-	//
-	updateLinksDofOffsets();
-}
-
-void btMultiBody::finalizeMultiDof()
-{
-	m_deltaV.resize(0);
-	m_deltaV.resize(6 + m_dofCount);
-	m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount);			//m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
-	m_vectorBuf.resize(2 * m_dofCount);													//two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
-	for (int i=0;i<m_vectorBuf.size();i++)
-	{
-		m_vectorBuf[i].setValue(0,0,0);
-	}
-	updateLinksDofOffsets();
-}
-	
-int btMultiBody::getParent(int i) const
-{
-    return m_links[i].m_parent;
-}
-
-btScalar btMultiBody::getLinkMass(int i) const
-{
-    return m_links[i].m_mass;
-}
-
-const btVector3 & btMultiBody::getLinkInertia(int i) const
-{
-    return m_links[i].m_inertiaLocal;
-}
-
-btScalar btMultiBody::getJointPos(int i) const
-{
-    return m_links[i].m_jointPos[0];
-}
-
-btScalar btMultiBody::getJointVel(int i) const
-{
-    return m_realBuf[6 + m_links[i].m_dofOffset];
-}
-
-btScalar * btMultiBody::getJointPosMultiDof(int i)
-{
-	return &m_links[i].m_jointPos[0];
-}
-
-btScalar * btMultiBody::getJointVelMultiDof(int i)
-{
-	return &m_realBuf[6 + m_links[i].m_dofOffset];
-}
-
-const btScalar * btMultiBody::getJointPosMultiDof(int i) const 
-{
-	return &m_links[i].m_jointPos[0];
-}
-
-const btScalar * btMultiBody::getJointVelMultiDof(int i) const 
-{
-	return &m_realBuf[6 + m_links[i].m_dofOffset];
-}
-
-
-void btMultiBody::setJointPos(int i, btScalar q)
-{
-    m_links[i].m_jointPos[0] = q;
-    m_links[i].updateCacheMultiDof();
-}
-
-void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
-{
-	for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
-		m_links[i].m_jointPos[pos] = q[pos];
-
-    m_links[i].updateCacheMultiDof();
-}
-
-void btMultiBody::setJointVel(int i, btScalar qdot)
-{
-    m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
-}
-
-void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot)
-{
-	for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof];
-}
-
-const btVector3 & btMultiBody::getRVector(int i) const
-{
-    return m_links[i].m_cachedRVector;
-}
-
-const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
-{
-    return m_links[i].m_cachedRotParentToThis;
-}
-
-btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
-{
-	btAssert(i>=-1);
-	btAssert(i<m_links.size());
-	if ((i<-1) || (i>=m_links.size()))
-	{
-		return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
-	}
-
-    btVector3 result = local_pos;
-    while (i != -1) {
-        // 'result' is in frame i. transform it to frame parent(i)
-        result += getRVector(i);
-        result = quatRotate(getParentToLocalRot(i).inverse(),result);
-        i = getParent(i);
-    }
-
-    // 'result' is now in the base frame. transform it to world frame
-    result = quatRotate(getWorldToBaseRot().inverse() ,result);
-    result += getBasePos();
-
-    return result;
-}
-
-btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
-{
-	btAssert(i>=-1);
-	btAssert(i<m_links.size());
-	if ((i<-1) || (i>=m_links.size()))
-	{
-		return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
-	}
-
-    if (i == -1) {
-        // world to base
-        return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
-    } else {
-        // find position in parent frame, then transform to current frame
-        return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
-    }
-}
-
-btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
-{
-	btAssert(i>=-1);
-	btAssert(i<m_links.size());
-	if ((i<-1) || (i>=m_links.size()))
-	{
-		return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
-	}
-
-
-    btVector3 result = local_dir;
-    while (i != -1) {
-        result = quatRotate(getParentToLocalRot(i).inverse() , result);
-        i = getParent(i);
-    }
-    result = quatRotate(getWorldToBaseRot().inverse() , result);
-    return result;
-}
-
-btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
-{
-	btAssert(i>=-1);
-	btAssert(i<m_links.size());
-	if ((i<-1) || (i>=m_links.size()))
-	{
-		return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
-	}
-
-    if (i == -1) {
-        return quatRotate(getWorldToBaseRot(), world_dir);
-    } else {
-        return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir));
-    }
-}
-
-btMatrix3x3 btMultiBody::localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
-{
-    btMatrix3x3 result = local_frame;
-    btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0));
-    btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1));
-    btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2));
-    result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
-    return result;
-}
-
-void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
-{
-	int num_links = getNumLinks();
-    // Calculates the velocities of each link (and the base) in its local frame
-    omega[0] = quatRotate(m_baseQuat ,getBaseOmega());
-    vel[0] = quatRotate(m_baseQuat ,getBaseVel());
-    
-    for (int i = 0; i < num_links; ++i) 
-	{
-        const int parent = m_links[i].m_parent;
-
-        // transform parent vel into this frame, store in omega[i+1], vel[i+1]
-        SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector,
-                         omega[parent+1], vel[parent+1],
-                         omega[i+1], vel[i+1]);
-
-        // now add qidot * shat_i
-		//only supported for revolute/prismatic joints, todo: spherical and planar joints
-		switch(m_links[i].m_jointType)
-		{
-			case btMultibodyLink::ePrismatic:
-			case btMultibodyLink::eRevolute:
-			{
-				btVector3 axisTop = m_links[i].getAxisTop(0);
-				btVector3 axisBottom = m_links[i].getAxisBottom(0);
-				btScalar jointVel = getJointVel(i);
-				omega[i+1] += jointVel * axisTop;
-				vel[i+1] += jointVel * axisBottom;
-				break;
-			}
-			default:
-			{
-			}
-		}
-	}
-}
-
-btScalar btMultiBody::getKineticEnergy() const
-{
-	int num_links = getNumLinks();
-    // TODO: would be better not to allocate memory here
-    btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
-	btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
-    compTreeLinkVelocities(&omega[0], &vel[0]);
-
-    // we will do the factor of 0.5 at the end
-    btScalar result = m_baseMass * vel[0].dot(vel[0]);
-    result += omega[0].dot(m_baseInertia * omega[0]);
-    
-    for (int i = 0; i < num_links; ++i) {
-        result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
-        result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]);
-    }
-
-    return 0.5f * result;
-}
-
-btVector3 btMultiBody::getAngularMomentum() const
-{
-	int num_links = getNumLinks();
-    // TODO: would be better not to allocate memory here
-    btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
-	btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
-    btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1);
-    compTreeLinkVelocities(&omega[0], &vel[0]);
-
-    rot_from_world[0] = m_baseQuat;
-    btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0]));
-
-    for (int i = 0; i < num_links; ++i) {
-        rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1];
-        result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
-    }
-
-    return result;
-}
-
-void btMultiBody::clearConstraintForces()
-{
-	m_baseConstraintForce.setValue(0, 0, 0);
-	m_baseConstraintTorque.setValue(0, 0, 0);
-
-
-    for (int i = 0; i < getNumLinks(); ++i) {
-        m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
-        m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
-    }
-}
-void btMultiBody::clearForcesAndTorques()
-{
-    m_baseForce.setValue(0, 0, 0);
-    m_baseTorque.setValue(0, 0, 0);
-
-	
-    for (int i = 0; i < getNumLinks(); ++i) {
-        m_links[i].m_appliedForce.setValue(0, 0, 0);
-        m_links[i].m_appliedTorque.setValue(0, 0, 0);
-		m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f;
-    }
-}
-
-void btMultiBody::clearVelocities()
-{
-	for (int i = 0; i < 6 + getNumDofs(); ++i) 
-	{
-		m_realBuf[i] = 0.f;
-	}
-}
-void btMultiBody::addLinkForce(int i, const btVector3 &f)
-{
-    m_links[i].m_appliedForce += f;
-}
-
-void btMultiBody::addLinkTorque(int i, const btVector3 &t)
-{
-    m_links[i].m_appliedTorque += t;
-}
-
-void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f)
-{
-    m_links[i].m_appliedConstraintForce += f;
-}
-
-void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t)
-{
-    m_links[i].m_appliedConstraintTorque += t;
-}
-
-
-
-void btMultiBody::addJointTorque(int i, btScalar Q)
-{
-    m_links[i].m_jointTorque[0] += Q;
-}
-
-void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q)
-{
-	m_links[i].m_jointTorque[dof] += Q;
-}
-
-void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q)
-{
-	for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		m_links[i].m_jointTorque[dof] = Q[dof];
-}
-
-const btVector3 & btMultiBody::getLinkForce(int i) const
-{
-    return m_links[i].m_appliedForce;
-}
-
-const btVector3 & btMultiBody::getLinkTorque(int i) const
-{
-    return m_links[i].m_appliedTorque;
-}
-
-btScalar btMultiBody::getJointTorque(int i) const
-{
-    return m_links[i].m_jointTorque[0];
-}
-
-btScalar * btMultiBody::getJointTorqueMultiDof(int i)
-{
-    return &m_links[i].m_jointTorque[0];
-}
-
-inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1)				//renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?
-{
-		btVector3 row0 = btVector3( 
-			v0.x() * v1.x(),
-			v0.x() * v1.y(),
-			v0.x() * v1.z());
-		btVector3 row1 = btVector3( 
-			v0.y() * v1.x(),
-			v0.y() * v1.y(),
-			v0.y() * v1.z());
-		btVector3 row2 = btVector3( 
-			v0.z() * v1.x(),
-			v0.z() * v1.y(),
-			v0.z() * v1.z());
-
-        btMatrix3x3 m(row0[0],row0[1],row0[2],
-						row1[0],row1[1],row1[2],
-						row2[0],row2[1],row2[2]);
-		return m;
-}
-
-#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
-//
-
-void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
-                               btAlignedObjectArray<btScalar> &scratch_r,
-                               btAlignedObjectArray<btVector3> &scratch_v,
-                               btAlignedObjectArray<btMatrix3x3> &scratch_m,
-				bool isConstraintPass)
-{
-    // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
-    // and the base linear & angular accelerations.
-
-    // We apply damping forces in this routine as well as any external forces specified by the 
-    // caller (via addBaseForce etc).
-
-    // output should point to an array of 6 + num_links reals.
-    // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
-    // num_links joint acceleration values.
-    
-	// We added support for multi degree of freedom (multi dof) joints.
-	// In addition we also can compute the joint reaction forces. This is performed in a second pass,
-	// so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver)
-
-	m_internalNeedsJointFeedback = false;
-
-	int num_links = getNumLinks();
-
-    const btScalar DAMPING_K1_LINEAR = m_linearDamping;
-	const btScalar DAMPING_K2_LINEAR = m_linearDamping;
-
-	const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
-	const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
-
-    btVector3 base_vel = getBaseVel();
-    btVector3 base_omega = getBaseOmega();
-
-    // Temporary matrices/vectors -- use scratch space from caller
-    // so that we don't have to keep reallocating every frame
-
-    scratch_r.resize(2*m_dofCount + 6);				//multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
-    scratch_v.resize(8*num_links + 6);
-    scratch_m.resize(4*num_links + 4);
-
-	//btScalar * r_ptr = &scratch_r[0];
-    btScalar * output = &scratch_r[m_dofCount];  // "output" holds the q_double_dot results
-    btVector3 * v_ptr = &scratch_v[0];
-    
-    // vhat_i  (top = angular, bottom = linear part)	
-	btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;
-	v_ptr += num_links * 2 + 2;
-	//
-    // zhat_i^A    
-	btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
-	v_ptr += num_links * 2 + 2;
-	//
-    // chat_i  (note NOT defined for the base)    
-	btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
-	v_ptr += num_links * 2;
-	//
-    // Ihat_i^A.    
-	btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];
-
-    // Cached 3x3 rotation matrices from parent frame to this frame.
-    btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
-    btMatrix3x3 * rot_from_world = &scratch_m[0];
-
-    // hhat_i, ahat_i
-    // hhat is NOT stored for the base (but ahat is)    
-	btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
-	btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
-	v_ptr += num_links * 2 + 2;
-	//
-    // Y_i, invD_i
-    btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
-	btScalar * Y = &scratch_r[0];
-	//
-	//aux variables	
-	btSpatialMotionVector spatJointVel;					//spatial velocity due to the joint motion (i.e. without predecessors' influence)
-	btScalar D[36];										//"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do	
-	btScalar invD_times_Y[6];							//D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies	
-	btSpatialMotionVector result;							//holds results of the SolveImatrix op; it is a spatial motion vector (accel)
-	btScalar Y_minus_hT_a[6];							//Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough	
-	btSpatialForceVector spatForceVecTemps[6];				//6 temporary spatial force vectors
-	btSpatialTransformationMatrix fromParent;				//spatial transform from parent to child
-	btSymmetricSpatialDyad dyadTemp;						//inertia matrix temp
-	btSpatialTransformationMatrix fromWorld;
-	fromWorld.m_trnVec.setZero();
-	/////////////////
-
-    // ptr to the joint accel part of the output
-    btScalar * joint_accel = output + 6;
-
-    // Start of the algorithm proper.
-    
-    // First 'upward' loop.
-    // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
-
-    rot_from_parent[0] = btMatrix3x3(m_baseQuat);				//m_baseQuat assumed to be alias!?
-
-	//create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
-	spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
-
-    if (m_fixedBase) 
-	{        
-		zeroAccSpatFrc[0].setZero();
-    }
-	else 
-	{
-		btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
-		btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
-		//external forces		
-		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));	
-
-		//adding damping terms (only)
-		btScalar linDampMult = 1., angDampMult = 1.;
-		zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
-								   linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
-
-		//
-		//p += vhat x Ihat vhat - done in a simpler way
-		if (m_useGyroTerm)
-			zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));
-		//
-		zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
-    }
-
-
-	//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
-	spatInertia[0].setMatrix(	btMatrix3x3(0,0,0,0,0,0,0,0,0),
-								//
-								btMatrix3x3(m_baseMass, 0, 0,
-											0, m_baseMass, 0,
-											0, 0, m_baseMass),
-								//
-								btMatrix3x3(m_baseInertia[0], 0, 0,
-											0, m_baseInertia[1], 0,
-											0, 0, m_baseInertia[2])
-							);
-
-    rot_from_world[0] = rot_from_parent[0];
-
-	//
-    for (int i = 0; i < num_links; ++i) {		
-        const int parent = m_links[i].m_parent;
-        rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
-        rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
-
-		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
-		fromWorld.m_rotMat = rot_from_world[i+1];
-		fromParent.transform(spatVel[parent+1], spatVel[i+1]);
-
-		// now set vhat_i to its true value by doing
-        // vhat_i += qidot * shat_i			
-		if(!m_useGlobalVelocities)
-		{
-			spatJointVel.setZero();
-
-			for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
-				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
-
-			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
-			spatVel[i+1] += spatJointVel;
-
-			//
-			// vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
-			//spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
-
-		}
-		else
-		{
-			fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]);
-			fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
-		}
-
-		// we can now calculate chat_i 		
-		spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]);		
-
-        // calculate zhat_i^A
-		//
-		//external forces		
-		btVector3 linkAppliedForce = isConstraintPass? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
-		btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
- 
-		zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce ));
-	
-#if 0	
-		{
-
-			b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
-			i+1,
-			zeroAccSpatFrc[i+1].m_topVec[0],
-			zeroAccSpatFrc[i+1].m_topVec[1],
-			zeroAccSpatFrc[i+1].m_topVec[2],
-
-			zeroAccSpatFrc[i+1].m_bottomVec[0],
-			zeroAccSpatFrc[i+1].m_bottomVec[1],
-			zeroAccSpatFrc[i+1].m_bottomVec[2]);
-		}
-#endif
-		//
-		//adding damping terms (only)
-		btScalar linDampMult = 1., angDampMult = 1.;
-		zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().safeNorm()),
-									 linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().safeNorm()));
-		
-        // calculate Ihat_i^A
-		//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
-		spatInertia[i+1].setMatrix(	btMatrix3x3(0,0,0,0,0,0,0,0,0),
-									//
-									btMatrix3x3(m_links[i].m_mass, 0, 0,
-												0, m_links[i].m_mass, 0,
-												0, 0, m_links[i].m_mass),
-									//
-									btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
-												0, m_links[i].m_inertiaLocal[1], 0,
-												0, 0, m_links[i].m_inertiaLocal[2])
-								);
-		//
-		//p += vhat x Ihat vhat - done in a simpler way
-		if(m_useGyroTerm)
-			zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));			
-		//		
-		zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
-		//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
-		////clamp parent's omega
-		//btScalar parOmegaMod = temp.length();
-		//btScalar parOmegaModMax = 1000;
-		//if(parOmegaMod > parOmegaModMax)
-		//	temp *= parOmegaModMax / parOmegaMod;
-		//zeroAccSpatFrc[i+1].addLinear(temp);
-		//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
-		//temp = spatCoriolisAcc[i].getLinear();
-		//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
-		
-		
-
-		//printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
-		//printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());		
-		//printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
-    }
-	
-    // 'Downward' loop.
-    // (part of TreeForwardDynamics in Mirtich.)
-    for (int i = num_links - 1; i >= 0; --i)
-	{
-		const int parent = m_links[i].m_parent;
-		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
-
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
-			//
-			hDof = spatInertia[i+1] * m_links[i].m_axes[dof];
-			//
-			Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
-			- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
-			- spatCoriolisAcc[i].dot(hDof)
-			;
-		}
-
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{
-			btScalar *D_row = &D[dof * m_links[i].m_dofCount];			
-			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
-			{
-				btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
-				D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
-			}
-		}
-
-        btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
-		switch(m_links[i].m_jointType)
-		{
-			case btMultibodyLink::ePrismatic:
-			case btMultibodyLink::eRevolute:
-			{
-				invDi[0] = 1.0f / D[0];
-				break;
-			}
-			case btMultibodyLink::eSpherical:
-			case btMultibodyLink::ePlanar:
-			{
-				btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
-				btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
-
-				//unroll the loop?
-				for(int row = 0; row < 3; ++row)
-				{
-					for(int col = 0; col < 3; ++col)
-					{						
-						invDi[row * 3 + col] = invD3x3[row][col];
-					}
-				}
-
-				break;
-			}
-			default:
-			{
-			
-			}
-		}
-
-		//determine h*D^{-1}
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{
-			spatForceVecTemps[dof].setZero();
-
-			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
-			{				
-				btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
-				//				
-				spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
-			}
-		}
-
-		dyadTemp = spatInertia[i+1];
-
-		//determine (h*D^{-1}) * h^{T}
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{			
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
-			//
-			dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
-		}
-
-		fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add);
-        
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{
-			invD_times_Y[dof] = 0.f;
-
-			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
-			{
-				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];				
-			}	
-		}
-		
-		spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i];		
-
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{				
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
-			//
-			spatForceVecTemps[0] += hDof * invD_times_Y[dof];			
-		}
-		
-		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
-		
-		zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
-    }
-
-
-    // Second 'upward' loop
-    // (part of TreeForwardDynamics in Mirtich)
-
-    if (m_fixedBase) 
-	{
-        spatAcc[0].setZero();
-    } 
-	else 
-	{
-        if (num_links > 0) 
-		{
-			m_cachedInertiaValid = true;
-			m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
-			m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
-			m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
-			m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose();
-
-        }		
-		
-		solveImatrix(zeroAccSpatFrc[0], result);
-		spatAcc[0] = -result;
-    }
-	
-	
-    // now do the loop over the m_links
-    for (int i = 0; i < num_links; ++i) 
-	{
-		//	qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)
-		//	a = apar + cor + Sqdd
-		//or
-		//	qdd = D^{-1} * (Y - h^{T}*(apar+cor))
-		//	a = apar + Sqdd
-
-        const int parent = m_links[i].m_parent;
-		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
-
-		fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
-		
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
-			//			
-			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);			
-		}
-
-		btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
-		//D^{-1} * (Y - h^{T}*apar)
-		mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
-
-		spatAcc[i+1] += spatCoriolisAcc[i];		
-
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
-			spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
-
-		if (m_links[i].m_jointFeedback)
-		{
-			m_internalNeedsJointFeedback = true;
-
-			btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
-			btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
-
-			if (gJointFeedbackInJointFrame)
-			{
-				//shift the reaction forces to the joint frame
-				//linear (force) component is the same
-				//shift the angular (torque, moment) component using the relative position,  m_links[i].m_dVector
-				 angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
-			}
-			
-
-			if (gJointFeedbackInWorldSpace)
-			{
-				if (isConstraintPass)
-				{
- m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
-                                        m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
-				} else
-				{
-					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
-					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
-				}
-			} else
-			{
-				if (isConstraintPass)
-				{
-					  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;                        
-                                m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
-
-				}
-				else
-				{
-				m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
-				m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
-				}		
-			}	
-	}
-
-    }
-
-    // transform base accelerations back to the world frame.
-    btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
-	output[0] = omegadot_out[0];
-	output[1] = omegadot_out[1];
-	output[2] = omegadot_out[2];
-
-    btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
-	output[3] = vdot_out[0];
-	output[4] = vdot_out[1];
-	output[5] = vdot_out[2];
-
-	/////////////////
-	//printf("q = [");
-	//printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
-	//for(int link = 0; link < getNumLinks(); ++link)
-	//	for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
-	//		printf("%.6f ", m_links[link].m_jointPos[dof]);
-	//printf("]\n");
-	////
-	//printf("qd = [");
-	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
-	//	printf("%.6f ", m_realBuf[dof]);
-	//printf("]\n");
-	//printf("qdd = [");
-	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
-	//	printf("%.6f ", output[dof]);
-	//printf("]\n");
-	/////////////////
-
-    // Final step: add the accelerations (times dt) to the velocities.
-
-	if (!isConstraintPass)
-	{
-	if(dt > 0.)
-		applyDeltaVeeMultiDof(output, dt);
-
-	}
-	/////
-	//btScalar angularThres = 1;
-	//btScalar maxAngVel = 0.;		
-	//bool scaleDown = 1.;
-	//for(int link = 0; link < m_links.size(); ++link)
-	//{		
-	//	if(spatVel[link+1].getAngular().length() > maxAngVel)
-	//	{
-	//		maxAngVel = spatVel[link+1].getAngular().length();
-	//		scaleDown = angularThres / spatVel[link+1].getAngular().length();
-	//		break;
-	//	}		
-	//}
-
-	//if(scaleDown != 1.)
-	//{
-	//	for(int link = 0; link < m_links.size(); ++link)
-	//	{
-	//		if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)
-	//		{
-	//			for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
-	//				getJointVelMultiDof(link)[dof] *= scaleDown;
-	//		}
-	//	}
-	//}
-	/////
-
-	/////////////////////
-	if(m_useGlobalVelocities)
-	{
-		for (int i = 0; i < num_links; ++i) 
-		{
-			const int parent = m_links[i].m_parent;
-			//rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);    /// <- done
-			//rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];		/// <- done
-		
-			fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
-			fromWorld.m_rotMat = rot_from_world[i+1];			
-        
-			// vhat_i = i_xhat_p(i) * vhat_p(i)		
-			fromParent.transform(spatVel[parent+1], spatVel[i+1]);
-			//nice alternative below (using operator *) but it generates temps
-			/////////////////////////////////////////////////////////////
-
-			// now set vhat_i to its true value by doing
-			// vhat_i += qidot * shat_i			
-			spatJointVel.setZero();
-
-			for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
-				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
-		
-			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
-			spatVel[i+1] += spatJointVel;
-
-
-			fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity);
-			fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
-		}
-	}
-	
-}
-
-
-
-void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, btScalar result[6]) const
-{
-	int num_links = getNumLinks();
-	///solve I * x = rhs, so the result = invI * rhs
-    if (num_links == 0) 
-	{
-		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
-        result[0] = rhs_bot[0] / m_baseInertia[0];
-        result[1] = rhs_bot[1] / m_baseInertia[1];
-        result[2] = rhs_bot[2] / m_baseInertia[2];
-        result[3] = rhs_top[0] / m_baseMass;
-        result[4] = rhs_top[1] / m_baseMass;
-        result[5] = rhs_top[2] / m_baseMass;
-    } else 
-	{
-		if (!m_cachedInertiaValid)
-		{
-			for (int i=0;i<6;i++)
-			{
-				result[i] = 0.f;
-			}
-			return;
-		}
-		/// Special routine for calculating the inverse of a spatial inertia matrix
-		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
-		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
-		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
-		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
-		tmp = invIupper_right * m_cachedInertiaLowerRight;
-		btMatrix3x3 invI_upper_left = (tmp * Binv);
-		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
-		tmp = m_cachedInertiaTopLeft  * invI_upper_left;
-		tmp[0][0]-= 1.0;
-		tmp[1][1]-= 1.0;
-		tmp[2][2]-= 1.0;
-		btMatrix3x3 invI_lower_left = (Binv * tmp);
-
-		//multiply result = invI * rhs
-		{
-		  btVector3 vtop = invI_upper_left*rhs_top;
-		  btVector3 tmp;
-		  tmp = invIupper_right * rhs_bot;
-		  vtop += tmp;
-		  btVector3 vbot = invI_lower_left*rhs_top;
-		  tmp = invI_lower_right * rhs_bot;
-		  vbot += tmp;
-		  result[0] = vtop[0];
-		  result[1] = vtop[1];
-		  result[2] = vtop[2];
-		  result[3] = vbot[0];
-		  result[4] = vbot[1];
-		  result[5] = vbot[2];
-		}
-		
-    }
-}
-void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
-{
-	int num_links = getNumLinks();
-	///solve I * x = rhs, so the result = invI * rhs
-    if (num_links == 0) 
-	{
-		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
-		result.setAngular(rhs.getAngular() / m_baseInertia);
-		result.setLinear(rhs.getLinear() / m_baseMass);		
-    } else 
-	{
-		/// Special routine for calculating the inverse of a spatial inertia matrix
-		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
-		if (!m_cachedInertiaValid)
-		{
-			result.setLinear(btVector3(0,0,0));
-			result.setAngular(btVector3(0,0,0));
-			result.setVector(btVector3(0,0,0),btVector3(0,0,0));
-			return;
-		}
-		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
-		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
-		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
-		tmp = invIupper_right * m_cachedInertiaLowerRight;
-		btMatrix3x3 invI_upper_left = (tmp * Binv);
-		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
-		tmp = m_cachedInertiaTopLeft  * invI_upper_left;
-		tmp[0][0]-= 1.0;
-		tmp[1][1]-= 1.0;
-		tmp[2][2]-= 1.0;
-		btMatrix3x3 invI_lower_left = (Binv * tmp);
-
-		//multiply result = invI * rhs
-		{
-		  btVector3 vtop = invI_upper_left*rhs.getLinear();
-		  btVector3 tmp;
-		  tmp = invIupper_right * rhs.getAngular();
-		  vtop += tmp;
-		  btVector3 vbot = invI_lower_left*rhs.getLinear();
-		  tmp = invI_lower_right * rhs.getAngular();
-		  vbot += tmp;
-		  result.setVector(vtop, vbot);		  
-		}
-		
-    }
-}
-
-void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
-{
-	for (int row = 0; row < rowsA; row++)
-	{
-		for (int col = 0; col < colsB; col++)
-		{
-			pC[row * colsB + col] = 0.f;
-			for (int inner = 0; inner < rowsB; inner++)
-			{
-				pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
-			}
-		}
-	}
-}
-
-void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
-                                       btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
-{
-    // Temporary matrices/vectors -- use scratch space from caller
-    // so that we don't have to keep reallocating every frame
-
-	
-	int num_links = getNumLinks();	
-    scratch_r.resize(m_dofCount);
-    scratch_v.resize(4*num_links + 4);	    
-
-    btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0;
-    btVector3 * v_ptr = &scratch_v[0];
-
-    // zhat_i^A (scratch space)
-    btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
-	v_ptr += num_links * 2 + 2;
-
-    // rot_from_parent (cached from calcAccelerations)
-    const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
-
-    // hhat (cached), accel (scratch)
-    // hhat is NOT stored for the base (but ahat is) 
-	const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
-	btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
-	v_ptr += num_links * 2 + 2;
-
-    // Y_i (scratch), invD_i (cached)
-    const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
-	btScalar * Y = r_ptr; 
-	////////////////
-	//aux variables
-	btScalar invD_times_Y[6];							//D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
-	btSpatialMotionVector result;							//holds results of the SolveImatrix op; it is a spatial motion vector (accel)
-	btScalar Y_minus_hT_a[6];							//Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough	
-	btSpatialForceVector spatForceVecTemps[6];				//6 temporary spatial force vectors
-	btSpatialTransformationMatrix fromParent;	
-	/////////////////
-
-    // First 'upward' loop.
-    // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
-	
-	// Fill in zero_acc
-    // -- set to force/torque on the base, zero otherwise
-    if (m_fixedBase) 
-	{
-        zeroAccSpatFrc[0].setZero();
-    } else 
-	{	
-		//test forces
-		fromParent.m_rotMat = rot_from_parent[0];
-		fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]);
-    }
-    for (int i = 0; i < num_links; ++i) 
-	{
-		zeroAccSpatFrc[i+1].setZero();
-    }    
-
-	// 'Downward' loop.
-    // (part of TreeForwardDynamics in Mirtich.)
-    for (int i = num_links - 1; i >= 0; --i)
-	{
-		const int parent = m_links[i].m_parent;
-		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
-
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{
-			Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
-											- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
-											;
-		}
-
-		btVector3 in_top, in_bottom, out_top, out_bottom;
-		const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
-		
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{
-			invD_times_Y[dof] = 0.f;
-
-			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
-			{
-				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];				
-			}	
-		}
-		
-		 // Zp += pXi * (Zi + hi*Yi/Di)
-		spatForceVecTemps[0] = zeroAccSpatFrc[i+1];
-
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{
-			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
-			//
-			spatForceVecTemps[0] += hDof * invD_times_Y[dof];		
-		}
-		
-
-		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
-			
-		zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
-    }
-	
-	// ptr to the joint accel part of the output
-    btScalar * joint_accel = output + 6;
-
-
-    // Second 'upward' loop
-    // (part of TreeForwardDynamics in Mirtich)
-
-    if (m_fixedBase) 
-	{
-        spatAcc[0].setZero();
-    } 
-	else 
-	{
-		solveImatrix(zeroAccSpatFrc[0], result);
-		spatAcc[0] = -result;
-
-    }
-	
-    // now do the loop over the m_links
-    for (int i = 0; i < num_links; ++i)
-	{
-        const int parent = m_links[i].m_parent;
-		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
-
-		fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
-		
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{
-			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
-			//			
-			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
-		}
-
-		const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
-		mulMatrix(const_cast<btScalar*>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
-
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
-			spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];      
-    }
-
-    // transform base accelerations back to the world frame.
-    btVector3 omegadot_out;
-    omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
-	output[0] = omegadot_out[0];
-	output[1] = omegadot_out[1];
-	output[2] = omegadot_out[2];
-
-    btVector3 vdot_out;
-    vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();
-	output[3] = vdot_out[0];
-	output[4] = vdot_out[1];
-	output[5] = vdot_out[2];
-
-	/////////////////
-	//printf("delta = [");
-	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
-	//	printf("%.2f ", output[dof]);
-	//printf("]\n");
-	/////////////////
-}
-
-
-
-
-void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
-{	
-	int num_links = getNumLinks();
-    // step position by adding dt * velocity
-	//btVector3 v = getBaseVel();	
-    //m_basePos += dt * v;
-	//
-	btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
-	btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);			//note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
-	//	
-	pBasePos[0] += dt * pBaseVel[0];
-	pBasePos[1] += dt * pBaseVel[1];
-	pBasePos[2] += dt * pBaseVel[2];
-
-	///////////////////////////////
-	//local functor for quaternion integration (to avoid error prone redundancy)
-	struct
-	{
-		//"exponential map" based on btTransformUtil::integrateTransform(..)
-		void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
-		{
-			//baseBody	=>	quat is alias and omega is global coor
-			//!baseBody	=>	quat is alibi and omega is local coor	
-			
-			btVector3 axis;
-			btVector3 angvel;
-
-			if(!baseBody)			
-				angvel = quatRotate(quat, omega);				//if quat is not m_baseQuat, it is alibi => ok			
-			else
-				angvel = omega;
-		
-			btScalar fAngle = angvel.length(); 		
-			//limit the angular motion
-			if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
-			{
-				fAngle = btScalar(0.5)*SIMD_HALF_PI / dt;
-			}
-
-			if ( fAngle < btScalar(0.001) )
-			{
-				// use Taylor's expansions of sync function
-				axis   = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle );
-			}
-			else
-			{
-				// sync(fAngle) = sin(c*fAngle)/t
-				axis   = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle );
-			}
-			
-			if(!baseBody)				
-				quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat;			
-			else			
-				quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) ));
-				//equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();			
-		
-			quat.normalize();
-		}
-	} pQuatUpdateFun;
-	///////////////////////////////
-
-	//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
-	//	
-	btScalar *pBaseQuat = pq ? pq : m_baseQuat;	
-	btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];		//note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
-	//
-	btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
-	btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
-	pQuatUpdateFun(baseOmega, baseQuat, true, dt);
-	pBaseQuat[0] = baseQuat.x();
-	pBaseQuat[1] = baseQuat.y();
-	pBaseQuat[2] = baseQuat.z();
-	pBaseQuat[3] = baseQuat.w();
-
-
-	//printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
-	//printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
-	//printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
-
-	if(pq)		
-		pq += 7;
-	if(pqd)
-		pqd += 6;
-
-	// Finally we can update m_jointPos for each of the m_links
-    for (int i = 0; i < num_links; ++i) 
-	{
-		btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);		
-		btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
-
-		switch(m_links[i].m_jointType)
-		{
-			case btMultibodyLink::ePrismatic:
-			case btMultibodyLink::eRevolute:
-			{
-				btScalar jointVel = pJointVel[0];	
-				pJointPos[0] += dt * jointVel;
-				break;
-			}
-			case btMultibodyLink::eSpherical:
-			{
-				btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
-				btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
-				pQuatUpdateFun(jointVel, jointOri, false, dt);
-				pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
-				break;
-			}
-			case btMultibodyLink::ePlanar:
-			{
-				pJointPos[0] += dt * getJointVelMultiDof(i)[0];
-
-				btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
-				btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
-				pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
-				pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
-
-				break;
-			}
-			default:
-			{
-			}
-
-		}
-
-		m_links[i].updateCacheMultiDof(pq);
-
-		if(pq)		
-			pq += m_links[i].m_posVarCount;
-		if(pqd)
-			pqd += m_links[i].m_dofCount;
-    }
-}
-
-void btMultiBody::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
-{
-    // temporary space
-	int num_links = getNumLinks();
-	int m_dofCount = getNumDofs();
-    scratch_v.resize(3*num_links + 3);			//(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
-    scratch_m.resize(num_links + 1);
-
-    btVector3 * v_ptr = &scratch_v[0];
-    btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
-    btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
-	btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
-    btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
-
-    scratch_r.resize(m_dofCount);
-    btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
-
-    btMatrix3x3 * rot_from_world = &scratch_m[0];
-
-    const btVector3 p_minus_com_world = contact_point - m_basePos;
-	const btVector3 &normal_lin_world = normal_lin;							//convenience
-	const btVector3 &normal_ang_world = normal_ang;
-
-    rot_from_world[0] = btMatrix3x3(m_baseQuat);    
-    
-    // omega coeffients first.
-    btVector3 omega_coeffs_world;
-    omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
-	jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
-	jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
-	jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
-    // then v coefficients
-    jac[3] = normal_lin_world[0];
-    jac[4] = normal_lin_world[1];
-    jac[5] = normal_lin_world[2];
-
-	//create link-local versions of p_minus_com and normal
-	p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
-    n_local_lin[0] = rot_from_world[0] * normal_lin_world;
-	n_local_ang[0] = rot_from_world[0] * normal_ang_world;
-
-    // Set remaining jac values to zero for now.
-    for (int i = 6; i < 6 + m_dofCount; ++i) 
-	{
-        jac[i] = 0;
-    }
-
-    // Qdot coefficients, if necessary.
-    if (num_links > 0 && link > -1) {
-
-        // TODO: speed this up -- don't calculate for m_links we don't need.
-        // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
-        // which is resulting in repeated work being done...)
-
-        // calculate required normals & positions in the local frames.
-        for (int i = 0; i < num_links; ++i) {
-
-            // transform to local frame
-            const int parent = m_links[i].m_parent;
-            const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
-            rot_from_world[i+1] = mtx * rot_from_world[parent+1];
-
-            n_local_lin[i+1] = mtx * n_local_lin[parent+1];
-			n_local_ang[i+1] = mtx * n_local_ang[parent+1];
-            p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector;
-
-			// calculate the jacobian entry
-			switch(m_links[i].m_jointType)
-			{
-				case btMultibodyLink::eRevolute:
-				{
-					results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
-					results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
-					break;
-				}
-				case btMultibodyLink::ePrismatic:
-				{
-					results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
-					break;
-				}
-				case btMultibodyLink::eSpherical:
-				{
-					results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
-					results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1));
-					results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2));
-										
-					results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
-					results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1));
-					results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2));
-
-					break;
-				}
-				case btMultibodyLink::ePlanar:
-				{
-					results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
-					results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
-					results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
-
-					break;
-				}
-				default:
-				{
-				}
-			}
-            
-        }
-
-        // Now copy through to output.
-		//printf("jac[%d] = ", link);
-        while (link != -1) 
-		{
-			for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
-			{
-				jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
-				//printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
-			}
-            
-			link = m_links[link].m_parent;
-        }
-		//printf("]\n");
-    }
-}
-
-
-void btMultiBody::wakeUp()
-{
-    m_awake = true;
-}
-
-void btMultiBody::goToSleep()
-{
-    m_awake = false;
-}
-
-void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
-{
-	extern bool gDisableDeactivation;
-    if (!m_canSleep || gDisableDeactivation) 
-	{
-		m_awake = true;
-		m_sleepTimer = 0;
-		return;
-	}
-
-    // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
-    btScalar motion = 0;
-	{
-		for (int i = 0; i < 6 + m_dofCount; ++i) 		
-			motion += m_realBuf[i] * m_realBuf[i];
-	}
-	
-
-    if (motion < SLEEP_EPSILON) {
-        m_sleepTimer += timestep;
-        if (m_sleepTimer > SLEEP_TIMEOUT) {
-            goToSleep();
-        }
-    } else {
-        m_sleepTimer = 0;
-		if (!m_awake)
-			wakeUp();
-    }
-}
-
-
-void	btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
-{
-	
-	int num_links = getNumLinks();
-
-	// Cached 3x3 rotation matrices from parent frame to this frame.
-	btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0];
-
-	rot_from_parent[0] = btMatrix3x3(m_baseQuat);				//m_baseQuat assumed to be alias!?
-	
-	for (int i = 0; i < num_links; ++i) 
-	{
-		rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
-	}
-		
-	int nLinks = getNumLinks();
-	///base + num m_links
-	world_to_local.resize(nLinks+1);
-	local_origin.resize(nLinks+1);
-
-	world_to_local[0] = getWorldToBaseRot();
-	local_origin[0] = getBasePos();
-	
-	for (int k=0;k<getNumLinks();k++)
-	{
-		const int parent = getParent(k);
-		world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
-		local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
-	}
-
-	for (int link=0;link<getNumLinks();link++)
-	{
-		int index = link+1;
-
-		btVector3 posr = local_origin[index];
-		btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
-		btTransform tr;
-		tr.setIdentity();
-		tr.setOrigin(posr);
-		tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
-		getLink(link).m_cachedWorldTransform = tr;
-			
-	}
-
-}
-
-void	btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
-{
-	world_to_local.resize(getNumLinks()+1);
-	local_origin.resize(getNumLinks()+1);
-	
-	world_to_local[0] = getWorldToBaseRot();
-	local_origin[0] = getBasePos();
-	
-	if (getBaseCollider())
-	{
-		btVector3 posr = local_origin[0];
-		//	float pos[4]={posr.x(),posr.y(),posr.z(),1};
-		btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
-		btTransform tr;
-		tr.setIdentity();
-		tr.setOrigin(posr);
-		tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
-		
-		getBaseCollider()->setWorldTransform(tr);
-		
-	}
-	
-	for (int k=0;k<getNumLinks();k++)
-	{
-		const int parent = getParent(k);
-		world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
-		local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
-	}
-	
-	
-	for (int m=0;m<getNumLinks();m++)
-	{
-		btMultiBodyLinkCollider* col = getLink(m).m_collider;
-		if (col)
-		{
-			int link = col->m_link;
-			btAssert(link == m);
-			
-			int index = link+1;
-			
-			btVector3 posr = local_origin[index];
-			//			float pos[4]={posr.x(),posr.y(),posr.z(),1};
-			btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
-			btTransform tr;
-			tr.setIdentity();
-			tr.setOrigin(posr);
-			tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
-			
-			col->setWorldTransform(tr);
-		}
-	}
-}
-
-int	btMultiBody::calculateSerializeBufferSize()	const
-{
-	int sz = sizeof(btMultiBodyData);
-	return sz;
-}
-
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
-{
-		btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
-		getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
-		mbd->m_baseMass = this->getBaseMass();
-		getBaseInertia().serialize(mbd->m_baseInertia);
-		{
-			char* name = (char*) serializer->findNameForPointer(m_baseName);
-			mbd->m_baseName = (char*)serializer->getUniquePointer(name);
-			if (mbd->m_baseName)
-			{
-				serializer->serializeName(name);
-			}
-		}
-		mbd->m_numLinks = this->getNumLinks();
-		if (mbd->m_numLinks)
-		{
-			int sz = sizeof(btMultiBodyLinkData);
-			int numElem = mbd->m_numLinks;
-			btChunk* chunk = serializer->allocate(sz,numElem);
-			btMultiBodyLinkData* memPtr = (btMultiBodyLinkData*)chunk->m_oldPtr;
-			for (int i=0;i<numElem;i++,memPtr++)
-			{
-
-				memPtr->m_jointType = getLink(i).m_jointType;
-				memPtr->m_dofCount = getLink(i).m_dofCount;
-				memPtr->m_posVarCount = getLink(i).m_posVarCount;
-				
-				getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
-				memPtr->m_linkMass = getLink(i).m_mass;
-				memPtr->m_parentIndex = getLink(i).m_parent;
-				memPtr->m_jointDamping = getLink(i).m_jointDamping;
-				memPtr->m_jointFriction = getLink(i).m_jointFriction;
-				memPtr->m_jointLowerLimit = getLink(i).m_jointLowerLimit;
-				memPtr->m_jointUpperLimit = getLink(i).m_jointUpperLimit;
-				memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce;
-				memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity;
-
-				getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
-				getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
-				getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
-				btAssert(memPtr->m_dofCount<=3);
-				for (int dof = 0;dof<getLink(i).m_dofCount;dof++)
-				{
-					getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
-					getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
-					
-					memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
-					memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
-
-				}
-				int numPosVar = getLink(i).m_posVarCount;
-				for (int posvar = 0; posvar < numPosVar;posvar++)
-				{
-					memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
-				}
-				
-				
-				{
-					char* name = (char*) serializer->findNameForPointer(m_links[i].m_linkName);
-					memPtr->m_linkName = (char*)serializer->getUniquePointer(name);
-					if (memPtr->m_linkName)
-					{
-						serializer->serializeName(name);
-					}
-				}
-				{
-					char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName);
-					memPtr->m_jointName = (char*)serializer->getUniquePointer(name);
-					if (memPtr->m_jointName)
-					{
-						serializer->serializeName(name);
-					}
-				}
-				memPtr->m_linkCollider = (btCollisionObjectData*)serializer->getUniquePointer(getLink(i).m_collider);
-
-			}
-			serializer->finalizeChunk(chunk,btMultiBodyLinkDataName,BT_ARRAY_CODE,(void*) &m_links[0]);
-		}
-		mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
-
-		// Fill padding with zeros to appease msan.
-#ifdef BT_USE_DOUBLE_PRECISION
-		memset(mbd->m_padding, 0, sizeof(mbd->m_padding));
-#endif
-
-		return btMultiBodyDataName;
-}
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.h
deleted file mode 100644
index 655165ac18..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.h
+++ /dev/null
@@ -1,814 +0,0 @@
-/*
- * 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 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 linkIndex,            // 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 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;
-	}
-
-	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; 
-	}     // 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; 
-	}
-
-	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 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!?
-	}
-    void setBaseOmega(const btVector3 &omega) 
-	{ 
-		m_realBuf[0]=omega[0]; 
-		m_realBuf[1]=omega[1]; 
-		m_realBuf[2]=omega[2]; 
-	}
-
-
-    //
-    // 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, btScalar *q);
-    void setJointVelMultiDof(int i, btScalar *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]; 
-	}
-/*    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.
-
-
-    //
-    // transform vectors in local frame of link i to world frame (or vice versa)
-    //
-    btVector3 localPosToWorld(int i, const btVector3 &vec) const;
-    btVector3 localDirToWorld(int i, const btVector3 &vec) const;
-    btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
-    btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
-
-    //
-    // transform a frame in local coordinate to a frame in world coordinate
-    //
-    btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const;
-
-    //
-    // calculate kinetic energy and angular momentum
-    // useful for debugging.
-    //
-
-    btScalar getKineticEnergy() const;
-    btVector3 getAngularMomentum() 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=false
-		);
-
-///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);
-        }
-
-    // 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 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);
-
-
-    //
-    // 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)
-	{
-		m_canSleep = canSleep;
-	}
-
-	bool getCanSleep()const
-	{
-		return m_canSleep;
-	}
-
-    bool isAwake() const { return m_awake; }
-    void wakeUp();
-    void goToSleep();
-    void checkMotionAndSleepIfRequired(btScalar timestep);
-    
-	bool hasFixedBase() const
-	{
-		    return m_fixedBase;
-	}
-
-	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>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
-
-	void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
-
-	void	updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
-	
-	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;
-	}
-
-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(btScalar *pA, 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)
-    btQuaternion m_baseQuat;   // rotates world points into base frame
-
-    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_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;
-    btScalar m_sleepTimer;
-
-	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;
-	
-	///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
-	bool m_internalNeedsJointFeedback;
-};
-
-struct btMultiBodyLinkDoubleData
-{
-	btQuaternionDoubleData	m_zeroRotParentToThis;
-	btVector3DoubleData		m_parentComToThisComOffset;
-	btVector3DoubleData		m_thisPivotToThisComOffset;
-	btVector3DoubleData		m_jointAxisTop[6];
-	btVector3DoubleData		m_jointAxisBottom[6];
-
-	btVector3DoubleData		m_linkInertia;   // inertia of the base (in local frame; diagonal)
-	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_parentComToThisComOffset;
-	btVector3FloatData		m_thisPivotToThisComOffset;
-	btVector3FloatData		m_jointAxisTop[6];
-	btVector3FloatData		m_jointAxisBottom[6];
-	btVector3FloatData	m_linkInertia;   // inertia of the base (in local frame; diagonal)
-	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
-{
-	btTransformDoubleData m_baseWorldTransform;
-	btVector3DoubleData m_baseInertia;   // inertia of the base (in local frame; diagonal)
-	double	m_baseMass;
-
-	char	*m_baseName;
-	btMultiBodyLinkDoubleData	*m_links;
-	btCollisionObjectDoubleData	*m_baseCollider;
-	char	*m_paddingPtr;
-	int		m_numLinks;
-	char	m_padding[4];
-};
-
-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btMultiBodyFloatData
-{
-	char	*m_baseName;
-	btMultiBodyLinkFloatData	*m_links;
-	btCollisionObjectFloatData	*m_baseCollider;
-	btTransformFloatData m_baseWorldTransform;
-	btVector3FloatData m_baseInertia;   // inertia of the base (in local frame; diagonal)
-	
-	float	m_baseMass;
-	int		m_numLinks;
-};
-
-
-
-#endif
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
deleted file mode 100644
index d52852dd8e..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
+++ /dev/null
@@ -1,417 +0,0 @@
-#include "btMultiBodyConstraint.h"
-#include "BulletDynamics/Dynamics/btRigidBody.h"
-#include "btMultiBodyPoint2Point.h"				//for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
-
-
-
-btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
-	:m_bodyA(bodyA),
-	m_bodyB(bodyB),
-	m_linkA(linkA),
-	m_linkB(linkB),
-	m_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)
-{
-    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 = rb0->getInvMass() + constraintNormalAng.dot(vec);
-            }
-            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 = rb1->getInvMass() + constraintNormalAng.dot(vec);
-            }
-            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->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
-        }
-        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->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
-        }
-        
-        solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
-    }
-    
-    
-    ///warm starting (or zero if disabled)
-    /*
-     if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-     {
-     solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-     
-     if (solverConstraint.m_appliedImpulse)
-     {
-     if (multiBodyA)
-     {
-     btScalar impulse = solverConstraint.m_appliedImpulse;
-     btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-     multiBodyA->applyDeltaVee(deltaV,impulse);
-     applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
-     } else
-     {
-     if (rb0)
-					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-     }
-     if (multiBodyB)
-     {
-     btScalar impulse = solverConstraint.m_appliedImpulse;
-     btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-     multiBodyB->applyDeltaVee(deltaV,impulse);
-     applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
-     } else
-     {
-     if (rb1)
-					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
-     }
-     }
-     } else
-     */
-    
-    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;
-    
-}
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
deleted file mode 100644
index 83521b9501..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
+++ /dev/null
@@ -1,195 +0,0 @@
-/*
-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"
-
-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_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);
-
-public:
-
-	BT_DECLARE_ALIGNED_ALLOCATOR();
-
-	btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
-	virtual ~btMultiBodyConstraint();
-
-	void updateJacobianSizes();
-	void allocateJacobiansMultiDof();
-
-	//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;
-	}
-
-	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
-
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
deleted file mode 100644
index 1e2d074096..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ /dev/null
@@ -1,1429 +0,0 @@
-/*
-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 "btMultiBodyConstraintSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
-#include "btMultiBodyLinkCollider.h"
-
-#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
-#include "btMultiBodyConstraint.h"
-#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
-
-#include "LinearMath/btQuickprof.h"
-
-btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
-{
-	btScalar leastSquaredResidual  = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
-	
-	//solve featherstone non-contact constraints
-
-	//printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
-
-	for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
-	{
-		int index = iteration&1? j : m_multiBodyNonContactConstraints.size()-1-j;
-
-		btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
-		
-		btScalar residual = resolveSingleConstraintRowGeneric(constraint);
-		leastSquaredResidual += residual*residual;
-
-		if(constraint.m_multiBodyA) 
-			constraint.m_multiBodyA->setPosUpdated(false);
-		if(constraint.m_multiBodyB) 
-			constraint.m_multiBodyB->setPosUpdated(false);
-	}
-
-	//solve featherstone normal contact
-	for (int j0=0;j0<m_multiBodyNormalContactConstraints.size();j0++)
-	{
-		int index = j0;//iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
-
-		btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[index];
-		btScalar residual = 0.f;
-
-		if (iteration < infoGlobal.m_numIterations)
-		{
-			residual = resolveSingleConstraintRowGeneric(constraint);
-		}
-
-		leastSquaredResidual += residual*residual;
- 
-		if(constraint.m_multiBodyA) 
-			constraint.m_multiBodyA->setPosUpdated(false);
-		if(constraint.m_multiBodyB) 
-			constraint.m_multiBodyB->setPosUpdated(false);
-	}
-	
-	//solve featherstone frictional contact
-
-	for (int j1=0;j1<this->m_multiBodyFrictionContactConstraints.size();j1++)
-	{
-		if (iteration < infoGlobal.m_numIterations)
-		{
-			int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
-
-			btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
-			btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
-			//adjust friction limits here
-			if (totalImpulse>btScalar(0))
-			{
-				frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
-				frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
-				btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
-				leastSquaredResidual += residual*residual;
-
-				if(frictionConstraint.m_multiBodyA) 
-					frictionConstraint.m_multiBodyA->setPosUpdated(false);
-				if(frictionConstraint.m_multiBodyB) 
-					frictionConstraint.m_multiBodyB->setPosUpdated(false);
-			}
-		}
-	}
-	return leastSquaredResidual;
-}
-
-btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
-{
-	m_multiBodyNonContactConstraints.resize(0);
-	m_multiBodyNormalContactConstraints.resize(0);
-	m_multiBodyFrictionContactConstraints.resize(0);
-	m_data.m_jacobians.resize(0);
-	m_data.m_deltaVelocitiesUnitImpulse.resize(0);
-	m_data.m_deltaVelocities.resize(0);
-
-	for (int i=0;i<numBodies;i++)
-	{
-		const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]);
-		if (fcA)
-		{
-			fcA->m_multiBody->setCompanionId(-1);
-		}
-	}
-
-	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
-
-	return val;
-}
-
-void	btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
-{
-    for (int i = 0; i < ndof; ++i) 
-		m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
-}
-
-btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
-{
-
-	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
-	btScalar deltaVelADotn=0;
-	btScalar deltaVelBDotn=0;
-	btSolverBody* bodyA = 0;
-	btSolverBody* bodyB = 0;
-	int ndofA=0;
-	int ndofB=0;
-
-	if (c.m_multiBodyA)
-	{
-		ndofA  = c.m_multiBodyA->getNumDofs() + 6;
-		for (int i = 0; i < ndofA; ++i) 
-			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
-	} else if(c.m_solverBodyIdA >= 0)
-	{
-		bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
-		deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
-	}
-
-	if (c.m_multiBodyB)
-	{
-		ndofB  = c.m_multiBodyB->getNumDofs() + 6;
-		for (int i = 0; i < ndofB; ++i) 
-			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
-	} else if(c.m_solverBodyIdB >= 0)
-	{
-		bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
-		deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
-	}
-
-	
-	deltaImpulse	-=	deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
-	deltaImpulse	-=	deltaVelBDotn*c.m_jacDiagABInv;
-	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
-	
-	if (sum < c.m_lowerLimit)
-	{
-		deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
-		c.m_appliedImpulse = c.m_lowerLimit;
-	}
-	else if (sum > c.m_upperLimit) 
-	{
-		deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
-		c.m_appliedImpulse = c.m_upperLimit;
-	}
-	else
-	{
-		c.m_appliedImpulse = sum;
-	}
-
-	if (c.m_multiBodyA)
-	{
-		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
-		c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
-#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-	} else if(c.m_solverBodyIdA >= 0)
-	{
-		bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
-
-	}
-	if (c.m_multiBodyB)
-	{
-		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
-		c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
-#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-	} else if(c.m_solverBodyIdB >= 0)
-	{
-		bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
-	}
-	return deltaImpulse;
-}
-
-
-
-
-void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, 
-																 const btVector3& contactNormal,
-																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
-																 btScalar& relaxation,
-																 bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
-{
-			
-	BT_PROFILE("setupMultiBodyContactConstraint");
-	btVector3 rel_pos1;
-	btVector3 rel_pos2;
-
-	btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
-	btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
-
-	const btVector3& pos1 = cp.getPositionWorldOnA();
-	const btVector3& pos2 = cp.getPositionWorldOnB();
-
-	btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
-	btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
-
-	btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
-	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
-
-	if (bodyA)
-		rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
-	if (bodyB)
-		rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
-
-	relaxation = infoGlobal.m_sor;
-	
-	btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
-	
-	 //cfm = 1 /       ( dt * kp + kd )
-    //erp = dt * kp / ( dt * kp + kd )
-    
-    btScalar cfm;
-	btScalar erp;
-	if (isFriction)
-	{
-		cfm = infoGlobal.m_frictionCFM;
-		erp = infoGlobal.m_frictionERP;
-	} else
-	{
-		cfm = infoGlobal.m_globalCfm;
-		erp = infoGlobal.m_erp2;
-
-		if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
-		{
-			if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
-				cfm  = cp.m_contactCFM;
-			if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
-				erp = cp.m_contactERP;                
-		} else
-		{
-			if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
-			{
-				btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
-				if (denom < SIMD_EPSILON)
-				{
-					denom = SIMD_EPSILON;
-				}
-				cfm = btScalar(1) / denom; 
-				erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
-			}
-		}
-	}
-
-	cfm *= invTimeStep;
-
-	if (multiBodyA)
-	{
-		if (solverConstraint.m_linkA<0)
-		{
-			rel_pos1 = pos1 - multiBodyA->getBasePos();
-		} else
-		{
-			rel_pos1 = pos1 - 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 = m_data.m_deltaVelocities.size();
-			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
-			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
-		} else
-		{
-			btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
-		}
-
-		solverConstraint.m_jacAindex = m_data.m_jacobians.size();
-		m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
-		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
-		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
-
-		btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
-		multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
-		btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-		multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
-
-		btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
-		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-		solverConstraint.m_contactNormal1 = contactNormal;
-	} else
-	{
-		btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
-		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-		solverConstraint.m_contactNormal1 = contactNormal;
-		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
-	}
-
-	
-
-	if (multiBodyB)
-	{
-		if (solverConstraint.m_linkB<0)
-		{
-			rel_pos2 = pos2 - multiBodyB->getBasePos();
-		} else
-		{
-			rel_pos2 = pos2 - 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 = m_data.m_deltaVelocities.size();
-			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
-			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
-		}
-
-		solverConstraint.m_jacBindex = m_data.m_jacobians.size();
-
-		m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
-		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
-		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
-
-		multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
-		multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
-		
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);		
-		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
-		solverConstraint.m_contactNormal2 = -contactNormal;
-	
-	} else
-	{
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);		
-		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
-		solverConstraint.m_contactNormal2 = -contactNormal;
-	
-		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
-	}
-
-	{
-						
-		btVector3 vec;
-		btScalar denom0 = 0.f;
-		btScalar denom1 = 0.f;
-		btScalar* jacB = 0;
-		btScalar* jacA = 0;
-		btScalar* lambdaA =0;
-		btScalar* lambdaB =0;
-		int ndofA  = 0;
-		if (multiBodyA)
-		{
-			ndofA  = multiBodyA->getNumDofs() + 6;
-			jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
-			lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-			for (int i = 0; i < ndofA; ++i)
-			{
-				btScalar j = jacA[i] ;
-				btScalar l =lambdaA[i];
-				denom0 += j*l;
-			}
-		} else
-		{
-			if (rb0)
-			{
-				vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
-				denom0 = rb0->getInvMass() + contactNormal.dot(vec);
-			}
-		}
-		if (multiBodyB)
-		{
-			const int ndofB  = multiBodyB->getNumDofs() + 6;
-			jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
-			lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-			for (int i = 0; i < ndofB; ++i)
-			{
-				btScalar j = jacB[i] ;
-				btScalar l =lambdaB[i];
-				denom1 += j*l;
-			}
-
-		} else
-		{
-			if (rb1)
-			{
-				vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
-				denom1 = rb1->getInvMass() + contactNormal.dot(vec);
-			}
-		}
-
-		 
-
-		 btScalar d = denom0+denom1+cfm;
-		 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 restitution = 0.f;
-    btScalar distance = 0;
-    if (!isFriction)
-    {
-        distance = cp.getDistance()+infoGlobal.m_linearSlop;
-    } else
-    {
-        if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
-        {
-          distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal);
-        }
-    }
-  
-    
-	btScalar rel_vel = 0.f;
-	int ndofA  = 0;
-	int ndofB  = 0;
-	{
-
-		btVector3 vel1,vel2;
-		if (multiBodyA)
-		{
-			ndofA  = multiBodyA->getNumDofs() + 6;
-			btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
-			for (int i = 0; i < ndofA ; ++i) 
-				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
-		} else
-		{
-			if (rb0)
-			{
-				rel_vel += (rb0->getVelocityInLocalPoint(rel_pos1) + 
-							(rb0->getTotalTorque()*rb0->getInvInertiaTensorWorld()*infoGlobal.m_timeStep).cross(rel_pos1)+
-							rb0->getTotalForce()*rb0->getInvMass()*infoGlobal.m_timeStep).dot(solverConstraint.m_contactNormal1);
-			}
-		}
-		if (multiBodyB)
-		{
-			ndofB  = multiBodyB->getNumDofs() + 6;
-			btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
-			for (int i = 0; i < ndofB ; ++i) 
-				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-
-		} else
-		{
-			if (rb1)
-			{
-				rel_vel += (rb1->getVelocityInLocalPoint(rel_pos2)+
-					(rb1->getTotalTorque()*rb1->getInvInertiaTensorWorld()*infoGlobal.m_timeStep).cross(rel_pos2) +
-					rb1->getTotalForce()*rb1->getInvMass()*infoGlobal.m_timeStep).dot(solverConstraint.m_contactNormal2);
-			}
-		}
-
-		solverConstraint.m_friction = cp.m_combinedFriction;
-
-		if(!isFriction)
-		{
-			restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);	
-			if (restitution <= btScalar(0.))
-			{
-				restitution = 0.f;
-			}
-		}
-	}
-
-
-	///warm starting (or zero if disabled)
-	//disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
-	if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-	{
-		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-
-		if (solverConstraint.m_appliedImpulse)
-		{
-			if (multiBodyA)
-			{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-				multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
-				
-				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
-			} else
-			{
-				if (rb0)
-					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-			}
-			if (multiBodyB)
-			{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-				multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
-				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
-			} else
-			{
-				if (rb1)
-					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
-			}
-		}
-	} else
-	{
-		solverConstraint.m_appliedImpulse = 0.f;
-	}
-
-	solverConstraint.m_appliedPushImpulse = 0.f;
-
-	{
-
-		btScalar positionalError = 0.f;
-		btScalar velocityError = restitution - rel_vel;// * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
-		if (isFriction)
-		{
-			positionalError = -distance * erp/infoGlobal.m_timeStep;
-		} else
-		{
-    			if (distance>0)
-			{
-				positionalError = 0;
-				velocityError -= distance / infoGlobal.m_timeStep;
-
-			} else
-			{
-				positionalError = -distance * erp/infoGlobal.m_timeStep;
-			}
-		}
-
-		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
-		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
-
-		if(!isFriction)
-		{
-		//	if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-			{
-				//combine position and velocity into rhs
-				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_lowerLimit = 0;
-			solverConstraint.m_upperLimit = 1e10f;
-		}
-		else
-		{
-			solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
-			solverConstraint.m_rhsPenetration = 0.f;
-			solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
-			solverConstraint.m_upperLimit = solverConstraint.m_friction;
-		}
-
-		solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
-		
-
-
-	}
-
-}
-
-void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
-                                                                  const btVector3& constraintNormal,
-                                                                  btManifoldPoint& cp,
-                                                                    btScalar combinedTorsionalFriction,
-                                                                    const btContactSolverInfo& infoGlobal,
-                                                                  btScalar& relaxation,
-                                                                  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
-{
-    
-    BT_PROFILE("setupMultiBodyRollingFrictionConstraint");
-    btVector3 rel_pos1;
-    btVector3 rel_pos2;
-    
-    btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
-    btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
-    
-    const btVector3& pos1 = cp.getPositionWorldOnA();
-    const btVector3& pos2 = cp.getPositionWorldOnB();
-    
-    btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
-    btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
-    
-    btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
-    btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
-    
-    if (bodyA)
-        rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
-    if (bodyB)
-        rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
-    
-    relaxation = infoGlobal.m_sor;
-    
-   // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
-    
-    
-    if (multiBodyA)
-    {
-        if (solverConstraint.m_linkA<0)
-        {
-            rel_pos1 = pos1 - multiBodyA->getBasePos();
-        } else
-        {
-            rel_pos1 = pos1 - 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 = m_data.m_deltaVelocities.size();
-            multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
-            m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
-        } else
-        {
-            btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
-        }
-        
-        solverConstraint.m_jacAindex = m_data.m_jacobians.size();
-        m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
-        m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
-        btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
-        
-        btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
-        multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0,0,0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
-        btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-        multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
-        
-        btVector3 torqueAxis0 = -constraintNormal;
-        solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-        solverConstraint.m_contactNormal1 = btVector3(0,0,0);
-    } else
-    {
-        btVector3 torqueAxis0 = -constraintNormal;
-        solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-        solverConstraint.m_contactNormal1 = btVector3(0,0,0);
-        solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
-    }
-    
-    
-    
-    if (multiBodyB)
-    {
-        if (solverConstraint.m_linkB<0)
-        {
-            rel_pos2 = pos2 - multiBodyB->getBasePos();
-        } else
-        {
-            rel_pos2 = pos2 - 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 = m_data.m_deltaVelocities.size();
-            multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
-            m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
-        }
-        
-        solverConstraint.m_jacBindex = m_data.m_jacobians.size();
-        
-        m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
-        m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
-        btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
-        
-        multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0,0,0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
-        multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
-        
-        btVector3 torqueAxis1 = constraintNormal;
-        solverConstraint.m_relpos2CrossNormal = torqueAxis1;
-        solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
-        
-    } else
-    {
-        btVector3 torqueAxis1 = constraintNormal;
-        solverConstraint.m_relpos2CrossNormal = torqueAxis1;
-        solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
-        
-        solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
-    }
-    
-    {
-        
-        btScalar denom0 = 0.f;
-        btScalar denom1 = 0.f;
-        btScalar* jacB = 0;
-        btScalar* jacA = 0;
-        btScalar* lambdaA =0;
-        btScalar* lambdaB =0;
-        int ndofA  = 0;
-        if (multiBodyA)
-        {
-            ndofA  = multiBodyA->getNumDofs() + 6;
-            jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
-            lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-            for (int i = 0; i < ndofA; ++i)
-            {
-                btScalar j = jacA[i] ;
-                btScalar l =lambdaA[i];
-                denom0 += j*l;
-            }
-        } else
-        {
-            if (rb0)
-            {
-				btVector3 iMJaA = rb0?rb0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
-				denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
-            }
-        }
-        if (multiBodyB)
-        {
-            const int ndofB  = multiBodyB->getNumDofs() + 6;
-            jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
-            lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-            for (int i = 0; i < ndofB; ++i)
-            {
-                btScalar j = jacB[i] ;
-                btScalar l =lambdaB[i];
-                denom1 += j*l;
-            }
-            
-        } else
-        {
-            if (rb1)
-            {
-				btVector3 iMJaB = rb1?rb1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
-				denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
-            }
-        }
-        
-        
-        
-        btScalar d = denom0+denom1+infoGlobal.m_globalCfm;
-        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 restitution = 0.f;
-    btScalar penetration = isFriction? 0 : cp.getDistance();
-    
-    btScalar rel_vel = 0.f;
-    int ndofA  = 0;
-    int ndofB  = 0;
-    {
-        
-        btVector3 vel1,vel2;
-        if (multiBodyA)
-        {
-            ndofA  = multiBodyA->getNumDofs() + 6;
-            btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
-            for (int i = 0; i < ndofA ; ++i)
-                rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
-        } else
-        {
-            if (rb0)
-            {
-				btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
-				rel_vel += solverConstraint.m_contactNormal1.dot(rb0?solverBodyA->m_linearVelocity+solverBodyA->m_externalForceImpulse:btVector3(0,0,0))
-				+ solverConstraint.m_relpos1CrossNormal.dot(rb0?solverBodyA->m_angularVelocity:btVector3(0,0,0));
-	
-            }
-        }
-        if (multiBodyB)
-        {
-            ndofB  = multiBodyB->getNumDofs() + 6;
-            btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
-            for (int i = 0; i < ndofB ; ++i)
-                rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-            
-        } else
-        {
-            if (rb1)
-            {
-				btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
-				rel_vel += solverConstraint.m_contactNormal2.dot(rb1?solverBodyB->m_linearVelocity+solverBodyB->m_externalForceImpulse:btVector3(0,0,0))
-			+ solverConstraint.m_relpos2CrossNormal.dot(rb1?solverBodyB->m_angularVelocity:btVector3(0,0,0));
-
-            }
-        }
-
-        solverConstraint.m_friction =combinedTorsionalFriction;
-        
-        if(!isFriction)
-        {
-            restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
-            if (restitution <= btScalar(0.))
-            {
-                restitution = 0.f;
-            }
-        }
-    }
-    
-    
-    solverConstraint.m_appliedImpulse = 0.f;
-    solverConstraint.m_appliedPushImpulse = 0.f;
-    
-    {
-        
-        btScalar velocityError = 0 - rel_vel;// * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
-        
-       
-        
-        btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv;
-        
-        solverConstraint.m_rhs = velocityImpulse;
-        solverConstraint.m_rhsPenetration = 0.f;
-        solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
-        solverConstraint.m_upperLimit = solverConstraint.m_friction;
-        
-        solverConstraint.m_cfm = infoGlobal.m_globalCfm*solverConstraint.m_jacDiagABInv;
-        
-        
-        
-    }
-    
-}
-
-btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
-{
-	BT_PROFILE("addMultiBodyFrictionConstraint");
-	btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
-    solverConstraint.m_orgConstraint = 0;
-    solverConstraint.m_orgDofIndex = -1;
-    
-	solverConstraint.m_frictionIndex = frictionIndex;
-	bool isFriction = true;
-
-	const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
-	const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
-	
-	btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
-	btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
-
-	int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
-	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
-
-	solverConstraint.m_solverBodyIdA = solverBodyIdA;
-	solverConstraint.m_solverBodyIdB = solverBodyIdB;
-	solverConstraint.m_multiBodyA = mbA;
-	if (mbA)
-		solverConstraint.m_linkA = fcA->m_link;
-
-	solverConstraint.m_multiBodyB = mbB;
-	if (mbB)
-		solverConstraint.m_linkB = fcB->m_link;
-
-	solverConstraint.m_originalContactPoint = &cp;
-
-	setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
-	return solverConstraint;
-}
-
-btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,
-                                                                btScalar combinedTorsionalFriction,
-                                                                                                     btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
-{
-    BT_PROFILE("addMultiBodyRollingFrictionConstraint");
-    btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
-    solverConstraint.m_orgConstraint = 0;
-    solverConstraint.m_orgDofIndex = -1;
-    
-    solverConstraint.m_frictionIndex = frictionIndex;
-    bool isFriction = true;
-    
-    const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
-    const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
-    
-    btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
-    btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
-    
-    int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
-    int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
-    
-    solverConstraint.m_solverBodyIdA = solverBodyIdA;
-    solverConstraint.m_solverBodyIdB = solverBodyIdB;
-    solverConstraint.m_multiBodyA = mbA;
-    if (mbA)
-        solverConstraint.m_linkA = fcA->m_link;
-    
-    solverConstraint.m_multiBodyB = mbB;
-    if (mbB)
-        solverConstraint.m_linkB = fcB->m_link;
-    
-    solverConstraint.m_originalContactPoint = &cp;
-    
-    setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction,infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
-    return solverConstraint;
-}
-
-void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
-{
-	const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
-	const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
-	
-	btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
-	btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
-
-	btCollisionObject* colObj0=0,*colObj1=0;
-
-	colObj0 = (btCollisionObject*)manifold->getBody0();
-	colObj1 = (btCollisionObject*)manifold->getBody1();
-
-	int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
-	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
-
-//	btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
-//	btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
-
-
-	///avoid collision response between two static objects
-//	if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
-	//	return;
-
-    //only a single rollingFriction per manifold
-    int rollingFriction=1;
-    
-	for (int j=0;j<manifold->getNumContacts();j++)
-	{
-
-		btManifoldPoint& cp = manifold->getContactPoint(j);
-
-		if (cp.getDistance() <= manifold->getContactProcessingThreshold())
-		{
-		
-			btScalar relaxation;
-
-			int frictionIndex = m_multiBodyNormalContactConstraints.size();
-
-			btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
-
-	//		btRigidBody* rb0 = btRigidBody::upcast(colObj0);
-	//		btRigidBody* rb1 = btRigidBody::upcast(colObj1);
-            solverConstraint.m_orgConstraint = 0;
-            solverConstraint.m_orgDofIndex = -1;
-			solverConstraint.m_solverBodyIdA = solverBodyIdA;
-			solverConstraint.m_solverBodyIdB = solverBodyIdB;
-			solverConstraint.m_multiBodyA = mbA;
-			if (mbA)
-				solverConstraint.m_linkA = fcA->m_link;
-
-			solverConstraint.m_multiBodyB = mbB;
-			if (mbB)
-				solverConstraint.m_linkB = fcB->m_link;
-
-			solverConstraint.m_originalContactPoint = &cp;
-
-			bool isFriction = false;
-			setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction);
-
-//			const btVector3& pos1 = cp.getPositionWorldOnA();
-//			const btVector3& pos2 = cp.getPositionWorldOnB();
-
-			/////setup the friction constraints
-#define ENABLE_FRICTION
-#ifdef ENABLE_FRICTION
-			solverConstraint.m_frictionIndex = frictionIndex;
-
-			///Bullet has several options to set the friction directions
-			///By default, each contact has only a single friction direction that is recomputed automatically every frame
-			///based on the relative linear velocity.
-			///If the relative velocity is zero, it will automatically compute a friction direction.
-			
-			///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
-			///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
-			///
-			///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
-			///
-			///The user can manually override the friction directions for certain contacts using a contact callback, 
-			///and set the cp.m_lateralFrictionInitialized to true
-			///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
-			///this will give a conveyor belt effect
-			///
-
-			btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
-			cp.m_lateralFrictionDir1.normalize();
-			cp.m_lateralFrictionDir2.normalize();
-
-            if (rollingFriction > 0 )
-             {
-                if (cp.m_combinedSpinningFriction>0)
-                {
-                    addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal);
-                }
-                if (cp.m_combinedRollingFriction>0)
-                {
-
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					
-					if (cp.m_lateralFrictionDir1.length()>0.001)
-	                    addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
-
-					if (cp.m_lateralFrictionDir2.length()>0.001)
-                    addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
-                }
-              rollingFriction--;
-            }
-			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
-			{/*
-				cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
-				btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
-				if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
-				{
-					cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
-					if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-					{
-						cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
-						cp.m_lateralFrictionDir2.normalize();//??
-						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
-					}
-
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
-				} else
-				*/
-				{
-					
-
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
-                    
-
-					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-					{
-						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
-					}
-
-					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
-					{
-						cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
-					}
-				}
-
-			} else
-			{
-				addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_frictionCFM);
-
-				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_frictionCFM);
-
-				//setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
-				//todo:
-				solverConstraint.m_appliedImpulse = 0.f;
-				solverConstraint.m_appliedPushImpulse = 0.f;
-			}
-		
-
-#endif //ENABLE_FRICTION
-
-		}
-	}
-}
-
-void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
-{
-	//btPersistentManifold* manifold = 0;
-
-	for (int i=0;i<numManifolds;i++)
-	{
-		btPersistentManifold* manifold= manifoldPtr[i];
-		const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
-		const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
-		if (!fcA && !fcB)
-		{
-			//the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
-			convertContact(manifold,infoGlobal);
-		} else
-		{
-			convertMultiBodyContact(manifold,infoGlobal);
-		}
-	}
-
-	//also convert the multibody constraints, if any
-
-	
-	for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
-	{
-		btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
-		m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
-		m_data.m_fixedBodyId = m_fixedBodyId;
-		
-		c->createConstraintRows(m_multiBodyNonContactConstraints,m_data,	infoGlobal);
-	}
-
-}
-
-
-
-btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
-{
-	return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
-}
-
-#if 0
-static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
-{
-	if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
-	{
-		//todo: get rid of those temporary memory allocations for the joint feedback
-		btAlignedObjectArray<btScalar> forceVector;
-		int numDofsPlusBase = 6+mb->getNumDofs();
-		forceVector.resize(numDofsPlusBase);
-		for (int i=0;i<numDofsPlusBase;i++)
-		{
-			forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
-		}
-		btAlignedObjectArray<btScalar> output;
-		output.resize(numDofsPlusBase);
-		bool applyJointFeedback = true;
-		mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
-	}
-}
-#endif
-
-
-void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
-{
-#if 1 
-	
-	//bod->addBaseForce(m_gravity * bod->getBaseMass());
-	//bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
-
-	if (c.m_orgConstraint)
-	{
-		c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse);
-	}
-	
-
-	if (c.m_multiBodyA)
-	{
-		
-		c.m_multiBodyA->setCompanionId(-1);
-		btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
-		btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
-		if (c.m_linkA<0)
-		{
-			c.m_multiBodyA->addBaseConstraintForce(force);
-			c.m_multiBodyA->addBaseConstraintTorque(torque);
-		} else
-		{
-			c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
-				//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
-			c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
-		}
-	}
-	
-	if (c.m_multiBodyB)
-	{
-		{
-			c.m_multiBodyB->setCompanionId(-1);
-			btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
-			btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
-			if (c.m_linkB<0)
-			{
-				c.m_multiBodyB->addBaseConstraintForce(force);
-				c.m_multiBodyB->addBaseConstraintTorque(torque);
-			} else
-			{
-				{
-					c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force);
-					//b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
-					c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque);
-				}
-					
-			}
-		}
-	}
-#endif
-
-#ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-
-	if (c.m_multiBodyA)
-	{
-		
-		if(c.m_multiBodyA->isMultiDof())
-		{
-			c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
-		}
-		else
-		{
-			c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
-		}
-	}
-	
-	if (c.m_multiBodyB)
-	{
-		if(c.m_multiBodyB->isMultiDof())
-		{
-			c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
-		}
-		else
-		{
-			c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
-		}
-	}
-#endif
-
-
-
-}
-
-btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
-{
-	BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
-	int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
-	
-
-	//write back the delta v to the multi bodies, either as applied impulse (direct velocity change) 
-	//or as applied force, so we can measure the joint reaction forces easier
-	for (int i=0;i<numPoolConstraints;i++)
-	{
-		btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
-		writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
-
-		writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
-
-		if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-		{
-			writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
-		}
-	}
-
-
-	for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
-	{
-		btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
-		writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
-	}
-
-	
-	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-	{
-		BT_PROFILE("warm starting write back");
-		for (int j=0;j<numPoolConstraints;j++)
-		{
-			const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
-			btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
-			btAssert(pt);
-			pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
-			pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
-			
-			//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
-			if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-			{
-				pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
-			}
-			//do a callback here?
-		}
-	}
-#if 0
-	//multibody joint feedback
-	{
-		BT_PROFILE("multi body joint feedback");
-		for (int j=0;j<numPoolConstraints;j++)
-		{
-			const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
-		
-			//apply the joint feedback into all links of the btMultiBody
-			//todo: double-check the signs of the applied impulse
-
-			if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
-			{
-				applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
-			}
-			if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
-			{
-				applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
-			}
-#if 0
-			if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
-			{
-				applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
-					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
-					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
-					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
-
-			}
-			if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
-			{
-				applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
-					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
-					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
-					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
-			}
-		
-			if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-			{
-				if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
-				{
-					applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
-						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
-						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
-						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
-				}
-
-				if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
-				{
-					applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
-						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
-						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
-						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
-				}
-			}
-#endif
-		}
-	
-		for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
-		{
-			const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
-			if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
-			{
-				applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
-			}
-			if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
-			{
-				applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
-			}
-		}
-	}
-
-	numPoolConstraints = m_multiBodyNonContactConstraints.size();
-
-#if 0
-	//@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
-	for (int i=0;i<numPoolConstraints;i++)
-	{
-		const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
-
-		btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
-		btJointFeedback* fb = constr->getJointFeedback();
-		if (fb)
-		{
-			fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
-			fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
-			fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
-			fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
-			
-		}
-
-		constr->internalSetAppliedImpulse(c.m_appliedImpulse);
-		if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
-		{
-			constr->setEnabled(false);
-		}
-
-	}
-#endif 
-#endif
-
-	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
-}
-
-
-void  btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
-{
-	//printf("solveMultiBodyGroup start\n");
-	m_tmpMultiBodyConstraints = multiBodyConstraints;
-	m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
-	
-	btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
-
-	m_tmpMultiBodyConstraints = 0;
-	m_tmpNumMultiBodyConstraints = 0;
-	
-
-}
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
deleted file mode 100644
index 489347d874..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/*
-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;
-
-	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);
-	
-
-	void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
-    
-    btMultiBodySolverConstraint&	addMultiBodyFrictionConstraint(const btVector3& normalAxis,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);
-
-	void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, 
-																 btScalar* jacA,btScalar* jacB,
-																 btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
-																 const btContactSolverInfo& infoGlobal);
-
-	void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, 
-																 const btVector3& contactNormal,
-																 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
-
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
deleted file mode 100644
index 9eacc22647..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ /dev/null
@@ -1,991 +0,0 @@
-/*
-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::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)
-				{
-					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);
-}
-
-
-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<btPersistentManifold*> m_manifolds;
-	btAlignedObjectArray<btTypedConstraint*> m_constraints;
-	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
-
-
-	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=(MultiBodyInplaceSolverIslandCallback& other)
-	{
-		btAssert(0);
-		(void)other;
-		return *this;
-	}
-
-	SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints,	int	numMultiBodyConstraints,	btIDebugDraw* debugDrawer)
-	{
-		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);
-	}
-
-	
-	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);
-		} 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++)
-					m_bodies.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_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
-				{
-					processConstraints();
-				} else
-				{
-					//printf("deferred\n");
-				}
-			}
-		}
-	}
-	void	processConstraints()
-	{
-
-		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);
-		m_bodies.resize(0);
-		m_manifolds.resize(0);
-		m_constraints.resize(0);
-		m_multiBodyConstraints.resize(0);
-	}
-
-};
-
-
-
-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::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)
-{
-	forwardKinematics();
-
-
-
-	BT_PROFILE("solveConstraints");
-	
-	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());
-	
-	/// solve all the constraints for this island
-	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
-
-#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;
-
-				{
-					if(!bod->isUsingRK4Integration())
-					{
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m);
-					}
-					else
-					{						
-						//
-						int numDofs = bod->getNumDofs() + 6;
-						int numPosVars = bod->getNumPosVars() + 7;
-						btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
-						//convenience
-						btScalar *pMem = &scratch_r2[0];
-						btScalar *scratch_q0 = pMem; pMem += numPosVars;
-						btScalar *scratch_qx = pMem; pMem += numPosVars;
-						btScalar *scratch_qd0 = pMem; pMem += numDofs;
-						btScalar *scratch_qd1 = pMem; pMem += numDofs;
-						btScalar *scratch_qd2 = pMem; pMem += numDofs;
-						btScalar *scratch_qd3 = pMem; pMem += numDofs;
-						btScalar *scratch_qdd0 = pMem; pMem += numDofs;
-						btScalar *scratch_qdd1 = pMem; pMem += numDofs;
-						btScalar *scratch_qdd2 = pMem; pMem += numDofs;
-						btScalar *scratch_qdd3 = pMem; pMem += numDofs;
-						btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
-
-						/////						
-						//copy q0 to scratch_q0 and qd0 to scratch_qd0
-						scratch_q0[0] = bod->getWorldToBaseRot().x();
-						scratch_q0[1] = bod->getWorldToBaseRot().y();
-						scratch_q0[2] = bod->getWorldToBaseRot().z();
-						scratch_q0[3] = bod->getWorldToBaseRot().w();
-						scratch_q0[4] = bod->getBasePos().x();
-						scratch_q0[5] = bod->getBasePos().y();
-						scratch_q0[6] = bod->getBasePos().z();
-						//
-						for(int link = 0; link < bod->getNumLinks(); ++link)
-						{
-							for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
-								scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];							
-						}
-						//
-						for(int dof = 0; dof < numDofs; ++dof)								
-							scratch_qd0[dof] = bod->getVelocityVector()[dof];
-						////
-						struct
-						{
-						    btMultiBody *bod;
-                            btScalar *scratch_qx, *scratch_q0;
-
-						    void operator()()
-						    {
-						        for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
-                                    scratch_qx[dof] = scratch_q0[dof];
-						    }
-						} pResetQx = {bod, scratch_qx, scratch_q0};
-						//
-						struct
-						{
-						    void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
-						    {
-						        for(int i = 0; i < size; ++i)
-                                    pVal[i] = pCurVal[i] + dt * pDer[i];
-						    }
-
-						} pEulerIntegrate;
-						//
-						struct
-                        {
-                            void operator()(btMultiBody *pBody, const btScalar *pData)
-                            {
-                                btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
-
-                                for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
-                                    pVel[i] = pData[i];
-
-                            }
-                        } pCopyToVelocityVector;
-						//
-                        struct
-						{
-						    void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
-						    {
-						        for(int i = 0; i < size; ++i)
-                                    pDst[i] = pSrc[start + i];
-						    }
-						} pCopy;
-						//
-
-						btScalar h = solverInfo.m_timeStep;
-						#define output &m_scratch_r[bod->getNumDofs()]
-						//calc qdd0 from: q0 & qd0	
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
-						pCopy(output, scratch_qdd0, 0, numDofs);
-						//calc q1 = q0 + h/2 * qd0
-						pResetQx();
-						bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
-						//calc qd1 = qd0 + h/2 * qdd0
-						pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
-						//
-						//calc qdd1 from: q1 & qd1
-						pCopyToVelocityVector(bod, scratch_qd1);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
-						pCopy(output, scratch_qdd1, 0, numDofs);
-						//calc q2 = q0 + h/2 * qd1
-						pResetQx();
-						bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
-						//calc qd2 = qd0 + h/2 * qdd1
-						pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
-						//
-						//calc qdd2 from: q2 & qd2
-						pCopyToVelocityVector(bod, scratch_qd2);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
-						pCopy(output, scratch_qdd2, 0, numDofs);
-						//calc q3 = q0 + h * qd2
-						pResetQx();
-						bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
-						//calc qd3 = qd0 + h * qdd2
-						pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
-						//
-						//calc qdd3 from: q3 & qd3
-						pCopyToVelocityVector(bod, scratch_qd3);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
-						pCopy(output, scratch_qdd3, 0, numDofs);
-
-						//
-						//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
-						//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)						
-						btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
-						btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
-						for(int i = 0; i < numDofs; ++i)
-						{
-							delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
-							delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);							
-							//delta_q[i] = h*scratch_qd0[i];
-							//delta_qd[i] = h*scratch_qdd0[i];
-						}
-						//
-						pCopyToVelocityVector(bod, scratch_qd0);
-						bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);						
-						//
-						if(!doNotUpdatePos)
-						{
-							btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
-							pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
-
-							for(int i = 0; i < numDofs; ++i)
-								pRealBuf[i] = delta_q[i];
-
-							//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
-							bod->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);
-						}
-						
-					}
-				}
-				
-#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-				bod->clearForcesAndTorques();
-#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-			}//if (!isSleeping)
-		}
-	}
-
-	clearMultiBodyConstraintForces();
-
-	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->isUsingRK4Integration())
-                                {
-									bool isConstraintPass = true;
-                                    bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass);
-                                }
-				}
-			}
-		}
-	}
-
-	for (int i=0;i<this->m_multiBodies.size();i++)
-	{
-		btMultiBody* bod = m_multiBodies[i];
-		bod->processDeltaVeeMultiDof2();
-	}
-
-}
-
-void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
-{
-	btDiscreteDynamicsWorld::integrateTransforms(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();
-
-				///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);
-				
-			} 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);
-				
-				getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
-
-
-				for (int m = 0; m<bod->getNumLinks(); m++)
-				{
-					
-					const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
-
-					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);
-					
-						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);
-					
-						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);
-					
-						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);
-
-	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);
-		}
-	}
-
-}
\ No newline at end of file
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
deleted file mode 100644
index c0c132bbba..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
+++ /dev/null
@@ -1,114 +0,0 @@
-/*
-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"
-
-#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	solveConstraints(btContactSolverInfo& solverInfo);
-	
-	virtual void	serializeMultiBodies(btSerializer* serializer);
-
-public:
-
-	btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
-
-	virtual ~btMultiBodyDynamicsWorld ();
-
-	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);
-
-	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);
-
-};
-#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
deleted file mode 100644
index 1f94117aa9..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
+++ /dev/null
@@ -1,211 +0,0 @@
-/*
-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),
-	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),
-	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)
-	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
-		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
-		}
-	}
-	return -1;
-}
-
-int btMultiBodyFixedConstraint::getIslandIdB() const
-{
-	if (m_rigidBodyB)
-		return m_rigidBodyB->getIslandTag();
-	if (m_bodyB)
-	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
-		{
-			col = m_bodyB->getLink(i).m_collider;
-			if (col)
-				return col->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);
-	}
-}
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h
deleted file mode 100644
index 036025136e..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h
+++ /dev/null
@@ -1,94 +0,0 @@
-/*
-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
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
deleted file mode 100644
index 5fdb7007d8..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
+++ /dev/null
@@ -1,184 +0,0 @@
-/*
-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),
-	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)
-	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
-		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
-		}
-	}
-	return -1;
-}
-
-int btMultiBodyGearConstraint::getIslandIdB() const
-{
-	if (m_bodyB)
-	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
-		{
-			col = m_bodyB->getLink(i).m_collider;
-			if (col)
-				return col->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];
-			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);
-				}
-			};
-			
-		}
-
-	}
-
-}
-
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h
deleted file mode 100644
index 0115de6241..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h
+++ /dev/null
@@ -1,117 +0,0 @@
-/*
-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
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
deleted file mode 100644
index 5c2fa8ed5b..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/*
-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
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
deleted file mode 100644
index 6d173b66a1..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
+++ /dev/null
@@ -1,205 +0,0 @@
-/*
-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),
-	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)
-	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
-		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
-		}
-	}
-	return -1;
-}
-
-int btMultiBodyJointLimitConstraint::getIslandIdB() const
-{
-	if(m_bodyB)
-	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
-		{
-			col = m_bodyB->getLink(i).m_collider;
-			if (col)
-				return col->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;
-			}
-		}
-	}
-
-}
-
-
-
-
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h
deleted file mode 100644
index 55b8d122b9..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
-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)
-	}
-
-};
-
-#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
-
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
deleted file mode 100644
index e0921178e9..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
+++ /dev/null
@@ -1,186 +0,0 @@
-/*
-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),
-	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),
-	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
-{
-	btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-	if (col)
-		return col->getIslandTag();
-	for (int i=0;i<m_bodyA->getNumLinks();i++)
-	{
-		if (m_bodyA->getLink(i).m_collider)
-			return m_bodyA->getLink(i).m_collider->getIslandTag();
-	}
-	return -1;
-}
-
-int btMultiBodyJointMotor::getIslandIdB() const
-{
-	btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-	if (col)
-		return col->getIslandTag();
-
-	for (int i=0;i<m_bodyB->getNumLinks();i++)
-	{
-		col = m_bodyB->getLink(i).m_collider;
-		if (col)
-			return col->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);
-				}
-			};
-			
-		}
-
-	}
-
-}
-
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
deleted file mode 100644
index 4063bed79a..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/*
-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
-
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h
deleted file mode 100644
index 01828e5843..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h
+++ /dev/null
@@ -1,244 +0,0 @@
-/*
-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.
-
-    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];
-    
-    //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_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_appliedForce.setValue( 0, 0, 0);
-		m_appliedTorque.setValue(0, 0, 0);
-		//		
-		m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
-		m_jointPos[3] = 1.f;			//"quat.w"
-		m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
-		m_cachedWorldTransform.setIdentity();
-	}
-
-    // routine to update m_cachedRotParentToThis and m_cachedRVector
-	void updateCacheMultiDof(btScalar *pq = 0)
-	{
-		btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
-
-		switch(m_jointType)
-		{
-			case eRevolute:
-			{
-				m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
-
-				break;
-			}
-			case ePrismatic:
-			{
-				// m_cachedRotParentToThis never changes, so no need to update
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
-
-				break;
-			}
-			case eSpherical:
-			{
-				m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
-
-				break;
-			}
-			case ePlanar:
-			{
-				m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;				
-				m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector);				
-
-				break;
-			}
-			case eFixed:
-			{
-				m_cachedRotParentToThis = m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
-
-				break;
-			}
-			default:
-			{
-				//invalid type
-				btAssert(0);
-			}
-		}
-	}
-};
-
-
-#endif //BT_MULTIBODY_LINK_H
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
deleted file mode 100644
index 671e15d314..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
+++ /dev/null
@@ -1,125 +0,0 @@
-/*
-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"
-
-class btMultiBodyLinkCollider : public btCollisionObject
-{
-//protected:
-public:
-
-	btMultiBody* m_multiBody;
-	int m_link;
-
-
-	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;
-	}
-};
-
-#endif //BT_FEATHERSTONE_LINK_COLLIDER_H
-
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
deleted file mode 100644
index 125d52ad0b..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
+++ /dev/null
@@ -1,221 +0,0 @@
-/*
-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),
-	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),
-	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)
-	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
-		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
-		}
-	}
-	return -1;
-}
-
-int btMultiBodyPoint2Point::getIslandIdB() const
-{
-	if (m_rigidBodyB)
-		return m_rigidBodyB->getIslandTag();
-	if (m_bodyB)
-	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
-		{
-			col = m_bodyB->getLink(i).m_collider;
-			if (col)
-				return col->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);
-	}
-}
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
deleted file mode 100644
index bf39acc5b9..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
-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
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
deleted file mode 100644
index 3b64b8183f..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
+++ /dev/null
@@ -1,230 +0,0 @@
-/*
-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),
-	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),
-	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)
-	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
-		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
-		}
-	}
-	return -1;
-}
-
-int btMultiBodySliderConstraint::getIslandIdB() const
-{
-	if (m_rigidBodyB)
-		return m_rigidBodyB->getIslandTag();
-	if (m_bodyB)
-	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
-		{
-			col = m_bodyB->getLink(i).m_collider;
-			if (col)
-				return col->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);
-	}
-}
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h
deleted file mode 100644
index 0a6cf3df12..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h
+++ /dev/null
@@ -1,105 +0,0 @@
-/*
-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
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h
deleted file mode 100644
index 6fa1550e9e..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h
+++ /dev/null
@@ -1,90 +0,0 @@
-/*
-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