Update to Bullet 2.82.

Docs/Urho3D.dox

@@ -101,7 +101,7 @@ Urho3D is greatly inspired by OGRE (http://www.ogre3d.org/) and Horde3D (http://
 Urho3D uses the following third-party libraries:
 
 - AngelScript 2.28.1 (http://www.angelcode.com/angelscript/)
-- Bullet 2.81 (http://www.bulletphysics.org/)
+- Bullet 2.82 (http://www.bulletphysics.org/)
 - Civetweb (http://sourceforge.net/projects/civetweb/)
 - FreeType 2.5.0 (http://www.freetype.org/)
 - GLEW 1.9.0 (http://glew.sourceforge.net/)

Readme.txt

@@ -68,7 +68,7 @@ Urho3D is greatly inspired by OGRE (http://www.ogre3d.org) and Horde3D
 
 Urho3D uses the following third-party libraries:
 - AngelScript 2.28.1 (http://www.angelcode.com/angelscript/)
-- Bullet 2.81 (http://www.bulletphysics.org/)
+- Bullet 2.82 (http://www.bulletphysics.org/)
 - Civetweb (http://sourceforge.net/projects/civetweb/)
 - FreeType 2.5.0 (http://www.freetype.org/)
 - GLEW 1.9.0 (http://glew.sourceforge.net/)

Source/ThirdParty/Bullet/CMakeLists.txt

@@ -7,12 +7,14 @@ file (GLOB CPP_FILES src/BulletCollision/BroadphaseCollision/*.cpp
     src/BulletCollision/Gimpact/*.cpp src/BulletCollision/NarrowPhaseCollision/*.cpp 
     src/BulletDynamics/Character/*.cpp src/BulletDynamics/ConstraintSolver/*.cpp
     src/BulletDynamics/Dynamics/*.cpp src/BulletDynamics/Vehicle/*.cpp src/BulletSoftBody/*.cpp
+    src/BulletDynamics/Featherstone/*.cpp src/BulletDynamics/MLCPSolvers/*.cpp
     src/LinearMath/*.cpp)
 file (GLOB H_FILES *.h src/BulletCollision/BroadphaseCollision/*.h
     src/BulletCollision/CollisionDispatch/*.h src/BulletCollision/CollisionShapes/*.h
     src/BulletCollision/Gimpact/*.h src/BulletCollision/NarrowPhaseCollision/*.h
     src/BulletDynamics/Character/*.h src/BulletDynamics/ConstraintSolver/*.h
     src/BulletDynamics/Dynamics/*.h src/BulletDynamics/Vehicle/*.h src/BulletSoftBody/*.h 
+    src/BulletDynamics/Featherstone/*.h src/BulletDynamics/MLCPSolvers/*.h
     src/LinearMath/*.h)
 set (SOURCE_FILES ${CPP_FILES} ${H_FILES})
 # Install dependency for Engine/Physics/PhysicsWorld.h and Engine/Physics/RigidBody.h

Source/ThirdParty/Bullet/VERSION

@@ -1 +1 @@
-2.81
+2.82

Source/ThirdParty/Bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp

@@ -53,7 +53,7 @@ btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
 
 void	btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
 {
-	if (pair.m_algorithm)
+	if (pair.m_algorithm && dispatcher)
 	{
 		{
 			pair.m_algorithm->~btCollisionAlgorithm();

Source/ThirdParty/Bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

@@ -96,6 +96,12 @@ class btHashedOverlappingPairCache : public btOverlappingPairCache
 	btOverlapFilterCallback* m_overlapFilterCallback;
 	bool		m_blockedForChanges;
 
+protected:
+	
+	btAlignedObjectArray<int>	m_hashTable;
+	btAlignedObjectArray<int>	m_next;
+	btOverlappingPairCallback*	m_ghostPairCallback;
+
 
 public:
 	btHashedOverlappingPairCache();
@@ -265,11 +271,6 @@ private:
 	virtual void	sortOverlappingPairs(btDispatcher* dispatcher);
 	
 
-protected:
-	
-	btAlignedObjectArray<int>	m_hashTable;
-	btAlignedObjectArray<int>	m_next;
-	btOverlappingPairCallback*	m_ghostPairCallback;
 	
 };
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -142,7 +142,8 @@ public:
 		CO_GHOST_OBJECT=4,
 		CO_SOFT_BODY=8,
 		CO_HF_FLUID=16,
-		CO_USER_TYPE=32
+		CO_USER_TYPE=32,
+		CO_FEATHERSTONE_LINK=64
 	};
 
 	enum AnisotropicFrictionFlags

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -36,7 +36,7 @@ subject to the following restrictions:
 #include "LinearMath/btSerializer.h"
 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
-
+#include "BulletCollision/Gimpact/btGImpactShape.h"
 //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
 
 
@@ -289,13 +289,19 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 
 		btConvexShape* convexShape = (btConvexShape*) collisionShape;
 		btVoronoiSimplexSolver	simplexSolver;
-#define USE_SUBSIMPLEX_CONVEX_CAST 1
-#ifdef USE_SUBSIMPLEX_CONVEX_CAST
-		btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
-#else
-		//btGjkConvexCast	convexCaster(castShape,convexShape,&simplexSolver);
+		btSubsimplexConvexCast subSimplexConvexCaster(castShape,convexShape,&simplexSolver);
+		
+		btGjkConvexCast	gjkConvexCaster(castShape,convexShape,&simplexSolver);
+		
 		//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
-#endif //#USE_SUBSIMPLEX_CONVEX_CAST
+		bool condition = true;
+		btConvexCast* convexCasterPtr = 0;
+		if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest)
+			convexCasterPtr = &subSimplexConvexCaster;
+		else
+			convexCasterPtr = &gjkConvexCaster;
+		
+		btConvexCast& convexCaster = *convexCasterPtr;
 
 		if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
 		{
@@ -327,34 +333,26 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 	} else {
 		if (collisionShape->isConcave())
 		{
-			//			BT_PROFILE("rayTestConcave");
-			if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
-			{
-				///optimized version for btBvhTriangleMeshShape
-				btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
-				btTransform worldTocollisionObject = colObjWorldTransform.inverse();
-				btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
-				btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
 
-				//ConvexCast::CastResult
+			//ConvexCast::CastResult
 				struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
 				{
 					btCollisionWorld::RayResultCallback* m_resultCallback;
 					const btCollisionObject*	m_collisionObject;
-					btTriangleMeshShape*	m_triangleMesh;
+					const btConcaveShape*	m_triangleMesh;
 
 					btTransform m_colObjWorldTransform;
 
 					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
-						btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,btTriangleMeshShape*	triangleMesh,const btTransform& colObjWorldTransform):
-					//@BP Mod
-					btTriangleRaycastCallback(from,to, resultCallback->m_flags),
-						m_resultCallback(resultCallback),
-						m_collisionObject(collisionObject),
-						m_triangleMesh(triangleMesh),
-						m_colObjWorldTransform(colObjWorldTransform)
-					{
-					}
+					btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,const btConcaveShape*	triangleMesh,const btTransform& colObjWorldTransform):
+						//@BP Mod
+						btTriangleRaycastCallback(from,to, resultCallback->m_flags),
+							m_resultCallback(resultCallback),
+							m_collisionObject(collisionObject),
+							m_triangleMesh(triangleMesh),
+							m_colObjWorldTransform(colObjWorldTransform)
+						{
+						}
 
 
 					virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
@@ -377,10 +375,28 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 
 				};
 
+			btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+			btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
+			btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
+
+			//			BT_PROFILE("rayTestConcave");
+			if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
+			{
+				///optimized version for btBvhTriangleMeshShape
+				btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
+				
 				BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),triangleMesh,colObjWorldTransform);
 				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
 				triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
-			} else
+			}
+			else if(collisionShape->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
+			{
+				btGImpactMeshShape* concaveShape = (btGImpactMeshShape*)collisionShape;
+
+				BridgeTriangleRaycastCallback	rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
+				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+				concaveShape->processAllTrianglesRay(&rcb,rayFromLocal,rayToLocal);
+			}else
 			{
 				//generic (slower) case
 				btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://bulletphysics.com/Bullet/
+Copyright (c) 2003-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.
@@ -18,13 +18,11 @@ subject to the following restrictions:
  * @mainpage Bullet Documentation
  *
  * @section intro_sec Introduction
- * Bullet Collision Detection & Physics SDK
- *
  * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ).
  *
  * The main documentation is Bullet_User_Manual.pdf, included in the source code distribution.
  * There is the Physics Forum for feedback and general Collision Detection and Physics discussions.
- * Please visit http://www.bulletphysics.com
+ * Please visit http://www.bulletphysics.org
  *
  * @section install_sec Installation
  *
@@ -32,7 +30,16 @@ subject to the following restrictions:
  * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list
  *
  * @subsection step2 Step 2: Building
- * Bullet main build system for all platforms is cmake, you can download http://www.cmake.org
+ * Bullet has multiple build systems, including premake, cmake and autotools. Premake and cmake support all platforms.
+ * Premake is included in the Bullet/build folder for Windows, Mac OSX and Linux. 
+ * Under Windows you can click on Bullet/build/vs2010.bat to create Microsoft Visual Studio projects. 
+ * On Mac OSX and Linux you can open a terminal and generate Makefile, codeblocks or Xcode4 projects:
+ * cd Bullet/build
+ * ./premake4_osx gmake or ./premake4_linux gmake or ./premake4_linux64 gmake or (for Mac) ./premake4_osx xcode4
+ * cd Bullet/build/gmake
+ * make
+ * 
+ * An alternative to premake is cmake. You can download cmake from http://www.cmake.org
  * cmake can autogenerate projectfiles for Microsoft Visual Studio, Apple Xcode, KDevelop and Unix Makefiles.
  * The easiest is to run the CMake cmake-gui graphical user interface and choose the options and generate projectfiles.
  * You can also use cmake in the command-line. Here are some examples for various platforms:

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp

@@ -0,0 +1,421 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-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 "btCompoundCompoundCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+
+btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
+
+btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
+:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
+m_sharedManifold(ci.m_manifold)
+{
+	m_ownsManifold = false;
+
+	void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16);
+	m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache();
+
+	const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
+	btAssert (col0ObjWrap->getCollisionShape()->isCompound());
+
+	const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
+	btAssert (col1ObjWrap->getCollisionShape()->isCompound());
+	
+	const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
+	m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
+
+	const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
+	m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
+	
+	
+}
+
+
+btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm()
+{
+	removeChildAlgorithms();
+	m_childCollisionAlgorithmCache->~btHashedSimplePairCache();
+	btAlignedFree(m_childCollisionAlgorithmCache);
+}
+
+void	btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray&	manifoldArray)
+{
+	int i;
+	btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
+	for (i=0;i<pairs.size();i++)
+	{
+		if (pairs[i].m_userPointer)
+		{
+			
+			((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray);
+		}
+	}
+}
+
+
+void	btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms()
+{
+	btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
+
+	int numChildren = pairs.size();
+	int i;
+	for (i=0;i<numChildren;i++)
+	{
+		if (pairs[i].m_userPointer)
+		{
+			btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
+			algo->~btCollisionAlgorithm();
+			m_dispatcher->freeCollisionAlgorithm(algo);
+		}
+	}
+	m_childCollisionAlgorithmCache->removeAllPairs();
+}
+
+struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
+{
+	int m_numOverlapPairs;
+
+
+	const btCollisionObjectWrapper* m_compound0ColObjWrap;
+	const btCollisionObjectWrapper* m_compound1ColObjWrap;
+	btDispatcher* m_dispatcher;
+	const btDispatcherInfo& m_dispatchInfo;
+	btManifoldResult*	m_resultOut;
+	
+	
+	class btHashedSimplePairCache*	m_childCollisionAlgorithmCache;
+	
+	btPersistentManifold*	m_sharedManifold;
+	
+	btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap,
+									const btCollisionObjectWrapper* compound0ObjWrap,
+									btDispatcher* dispatcher,
+									const btDispatcherInfo& dispatchInfo,
+									btManifoldResult*	resultOut,
+									btHashedSimplePairCache* childAlgorithmsCache,
+									btPersistentManifold*	sharedManifold)
+		:m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
+		m_childCollisionAlgorithmCache(childAlgorithmsCache),
+		m_sharedManifold(sharedManifold),
+		m_numOverlapPairs(0)
+	{
+
+	}
+
+
+
+	
+	void		Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1)
+	{
+		m_numOverlapPairs++;
+
+
+		int childIndex0 = leaf0->dataAsInt;
+		int childIndex1 = leaf1->dataAsInt;
+		
+
+		btAssert(childIndex0>=0);
+		btAssert(childIndex1>=0);
+
+
+		const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape());
+		btAssert(childIndex0<compoundShape0->getNumChildShapes());
+
+		const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape());
+		btAssert(childIndex1<compoundShape1->getNumChildShapes());
+
+		const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0);
+		const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1);
+
+		//backup
+		btTransform	orgTrans0 = m_compound0ColObjWrap->getWorldTransform();
+		const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0);
+		btTransform	newChildWorldTrans0 = orgTrans0*childTrans0 ;
+		
+		btTransform	orgTrans1 = m_compound1ColObjWrap->getWorldTransform();
+		const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1);
+		btTransform	newChildWorldTrans1 = orgTrans1*childTrans1 ;
+		
+
+		//perform an AABB check first
+		btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
+		childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
+		childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
+		
+		if (gCompoundCompoundChildShapePairCallback)
+		{
+			if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
+				return;
+		}
+
+		if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+		{
+			btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0);
+			btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1);
+			
+
+			btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
+
+			btCollisionAlgorithm* colAlgo = 0;
+
+			if (pair)
+			{
+				colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
+				
+			} else
+			{
+				colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold);
+				pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1);
+				btAssert(pair);
+				pair->m_userPointer = colAlgo;
+			}
+
+			btAssert(colAlgo);
+						
+			const btCollisionObjectWrapper* tmpWrap0 = 0;
+			const btCollisionObjectWrapper* tmpWrap1 = 0;
+
+			tmpWrap0 = m_resultOut->getBody0Wrap();
+			tmpWrap1 = m_resultOut->getBody1Wrap();
+
+			m_resultOut->setBody0Wrap(&compoundWrap0);
+			m_resultOut->setBody1Wrap(&compoundWrap1);
+
+			m_resultOut->setShapeIdentifiersA(-1,childIndex0);
+			m_resultOut->setShapeIdentifiersB(-1,childIndex1);
+
+
+			colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut);
+			
+			m_resultOut->setBody0Wrap(tmpWrap0);
+			m_resultOut->setBody1Wrap(tmpWrap1);
+			
+
+
+		}
+	}
+};
+
+
+static DBVT_INLINE bool		MyIntersect(	const btDbvtAabbMm& a,
+								  const btDbvtAabbMm& b, const btTransform& xform)
+{
+	btVector3 newmin,newmax;
+	btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
+	btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
+	return Intersect(a,newb);
+}
+
+
+static inline void		MycollideTT(	const btDbvtNode* root0,
+								  const btDbvtNode* root1,
+								  const btTransform& xform,
+								  btCompoundCompoundLeafCallback* callback)
+{
+
+		if(root0&&root1)
+		{
+			int								depth=1;
+			int								treshold=btDbvt::DOUBLE_STACKSIZE-4;
+			btAlignedObjectArray<btDbvt::sStkNN>	stkStack;
+			stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
+			stkStack[0]=btDbvt::sStkNN(root0,root1);
+			do	{
+				btDbvt::sStkNN	p=stkStack[--depth];
+				if(MyIntersect(p.a->volume,p.b->volume,xform))
+				{
+					if(depth>treshold)
+					{
+						stkStack.resize(stkStack.size()*2);
+						treshold=stkStack.size()-4;
+					}
+					if(p.a->isinternal())
+					{
+						if(p.b->isinternal())
+						{					
+							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]);
+							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]);
+							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]);
+							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]);
+						}
+						else
+						{
+							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b);
+							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b);
+						}
+					}
+					else
+					{
+						if(p.b->isinternal())
+						{
+							stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]);
+							stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]);
+						}
+						else
+						{
+							callback->Process(p.a,p.b);
+						}
+					}
+				}
+			} while(depth);
+		}
+}
+
+void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+	const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
+	const btCollisionObjectWrapper* col1ObjWrap= body1Wrap;
+
+	btAssert (col0ObjWrap->getCollisionShape()->isCompound());
+	btAssert (col1ObjWrap->getCollisionShape()->isCompound());
+	const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
+	const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
+
+	///btCompoundShape might have changed:
+	////make sure the internal child collision algorithm caches are still valid
+	if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
+	{
+		///clear all
+		removeChildAlgorithms();
+	}
+
+
+	///we need to refresh all contact manifolds
+	///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
+	///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
+	{
+		int i;
+		btManifoldArray manifoldArray;
+		btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
+		for (i=0;i<pairs.size();i++)
+		{
+			if (pairs[i].m_userPointer)
+			{
+				btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
+				algo->getAllContactManifolds(manifoldArray);
+				for (int m=0;m<manifoldArray.size();m++)
+				{
+					if (manifoldArray[m]->getNumContacts())
+					{
+						resultOut->setPersistentManifold(manifoldArray[m]);
+						resultOut->refreshContactPoints();
+						resultOut->setPersistentManifold(0);
+					}
+				}
+				manifoldArray.resize(0);
+			}
+		}
+	}
+
+
+	const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
+	const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
+
+	btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);
+
+
+	const btTransform	xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
+	MycollideTT(tree0->m_root,tree1->m_root,xform,&callback);
+
+	//printf("#compound-compound child/leaf overlap =%d                      \r",callback.m_numOverlapPairs);
+
+	//remove non-overlapping child pairs
+
+	{
+		btAssert(m_removePairs.size()==0);
+
+		//iterate over all children, perform an AABB check inside ProcessChildShape
+		btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
+		
+		int i;
+		btManifoldArray	manifoldArray;
+        
+		
+
+        
+        
+        btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;        
+        
+		for (i=0;i<pairs.size();i++)
+		{
+			if (pairs[i].m_userPointer)
+			{
+				btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
+
+				{
+					btTransform	orgTrans0;
+					const btCollisionShape* childShape0 = 0;
+					
+					btTransform	newChildWorldTrans0;
+					btTransform	orgInterpolationTrans0;
+					childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
+					orgTrans0 = col0ObjWrap->getWorldTransform();
+					orgInterpolationTrans0 = col0ObjWrap->getWorldTransform();
+					const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
+					newChildWorldTrans0 = orgTrans0*childTrans0 ;
+					childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
+				}
+
+				{
+					btTransform	orgInterpolationTrans1;
+					const btCollisionShape* childShape1 = 0;
+					btTransform	orgTrans1;
+					btTransform	newChildWorldTrans1;
+
+					childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
+					orgTrans1 = col1ObjWrap->getWorldTransform();
+					orgInterpolationTrans1 = col1ObjWrap->getWorldTransform();
+					const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
+					newChildWorldTrans1 = orgTrans1*childTrans1 ;
+					childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
+				}
+				
+				
+
+				if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+				{
+					algo->~btCollisionAlgorithm();
+					m_dispatcher->freeCollisionAlgorithm(algo);
+					m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB));
+				}
+			}
+		}
+		for (int i=0;i<m_removePairs.size();i++)
+		{
+			m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB);
+		}
+		m_removePairs.clear();
+	}
+
+}
+
+btScalar	btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	btAssert(0);
+	return 0.f;
+
+}
+
+
+

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h

@@ -0,0 +1,90 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-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_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
+#define BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "BulletCollision/CollisionDispatch/btHashedSimplePairCache.h"
+class btDispatcher;
+class btCollisionObject;
+
+class btCollisionShape;
+typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1);
+extern btShapePairCallback gCompoundCompoundChildShapePairCallback;
+
+/// btCompoundCompoundCollisionAlgorithm  supports collision between two btCompoundCollisionShape shapes
+class btCompoundCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
+{
+
+	class btHashedSimplePairCache*	m_childCollisionAlgorithmCache;
+	btSimplePairArray m_removePairs;
+
+	class btPersistentManifold*	m_sharedManifold;
+	bool					m_ownsManifold;
+
+
+	int	m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated
+	int	m_compoundShapeRevision1;
+	
+	void	removeChildAlgorithms();
+	
+//	void	preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+
+public:
+
+	btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
+
+	virtual ~btCompoundCompoundCollisionAlgorithm();
+
+	
+
+	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray);
+	
+	
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm));
+			return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false);
+		}
+	};
+
+	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm));
+			return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true);
+		}
+	};
+
+};
+
+#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -76,21 +76,27 @@ void	btConvexTriangleCallback::clearCache()
 }
 
 
-
-void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
+void btConvexTriangleCallback::processTriangle(btVector3* triangle,int
+partId, int triangleIndex)
 {
- 
-	//just for debugging purposes
-	//printf("triangle %d",m_triangleCount++);
 
+	if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax))
+	{
+		return;
+	}
+
+        //just for debugging purposes
+        //printf("triangle %d",m_triangleCount++);
 
-	//aabb filter is already applied!	
+        const btCollisionObject* ob = const_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
 
 	btCollisionAlgorithmConstructionInfo ci;
 	ci.m_dispatcher1 = m_dispatcher;
 
 	//const btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
 
+	
+
 
 #if 0	
 	///debug drawing of the overlapping triangles

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp

@@ -19,6 +19,8 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h"
+
 #include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
@@ -64,6 +66,10 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
 	m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc;
 	mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16);
 	m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc;
+
+	mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc),16);
+	m_compoundCompoundCreateFunc = new (mem)btCompoundCompoundCollisionAlgorithm::CreateFunc;
+
 	mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16);
 	m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc;
 	mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16);
@@ -155,6 +161,9 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
 	m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc();
 	btAlignedFree( m_compoundCreateFunc);
 
+	m_compoundCompoundCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree(m_compoundCompoundCreateFunc);
+
 	m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc();
 	btAlignedFree( m_swappedCompoundCreateFunc);
 
@@ -258,6 +267,12 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
 		return m_swappedConvexConcaveCreateFunc;
 	}
 
+
+	if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1))
+	{
+		return m_compoundCompoundCreateFunc;
+	}
+
 	if (btBroadphaseProxy::isCompound(proxyType0))
 	{
 		return m_compoundCreateFunc;

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h

@@ -69,6 +69,8 @@ protected:
 	btCollisionAlgorithmCreateFunc*	m_convexConcaveCreateFunc;
 	btCollisionAlgorithmCreateFunc*	m_swappedConvexConcaveCreateFunc;
 	btCollisionAlgorithmCreateFunc*	m_compoundCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_compoundCompoundCreateFunc;
+	
 	btCollisionAlgorithmCreateFunc*	m_swappedCompoundCreateFunc;
 	btCollisionAlgorithmCreateFunc* m_emptyCreateFunc;
 	btCollisionAlgorithmCreateFunc* m_sphereSphereCF;

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp

@@ -0,0 +1,278 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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 "btHashedSimplePairCache.h"
+
+
+#include <stdio.h>
+
+int	gOverlappingSimplePairs = 0;
+int gRemoveSimplePairs =0;
+int gAddedSimplePairs =0;
+int gFindSimplePairs =0;
+
+
+
+
+btHashedSimplePairCache::btHashedSimplePairCache():
+	m_blockedForChanges(false)
+{
+	int initialAllocatedSize= 2;
+	m_overlappingPairArray.reserve(initialAllocatedSize);
+	growTables();
+}
+
+
+
+
+btHashedSimplePairCache::~btHashedSimplePairCache()
+{
+}
+
+
+
+
+
+
+void btHashedSimplePairCache::removeAllPairs()
+{
+	m_overlappingPairArray.clear();
+	m_hashTable.clear();
+	m_next.clear();
+
+	int initialAllocatedSize= 2;
+	m_overlappingPairArray.reserve(initialAllocatedSize);
+	growTables();
+}
+
+
+
+btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB)
+{
+	gFindSimplePairs++;
+	
+	
+	/*if (indexA > indexB) 
+		btSwap(indexA, indexB);*/
+
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
+
+	if (hash >= m_hashTable.size())
+	{
+		return NULL;
+	}
+
+	int index = m_hashTable[hash];
+	while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], indexA, indexB) == false)
+	{
+		index = m_next[index];
+	}
+
+	if (index == BT_SIMPLE_NULL_PAIR)
+	{
+		return NULL;
+	}
+
+	btAssert(index < m_overlappingPairArray.size());
+
+	return &m_overlappingPairArray[index];
+}
+
+//#include <stdio.h>
+
+void	btHashedSimplePairCache::growTables()
+{
+
+	int newCapacity = m_overlappingPairArray.capacity();
+
+	if (m_hashTable.size() < newCapacity)
+	{
+		//grow hashtable and next table
+		int curHashtableSize = m_hashTable.size();
+
+		m_hashTable.resize(newCapacity);
+		m_next.resize(newCapacity);
+
+
+		int i;
+
+		for (i= 0; i < newCapacity; ++i)
+		{
+			m_hashTable[i] = BT_SIMPLE_NULL_PAIR;
+		}
+		for (i = 0; i < newCapacity; ++i)
+		{
+			m_next[i] = BT_SIMPLE_NULL_PAIR;
+		}
+
+		for(i=0;i<curHashtableSize;i++)
+		{
+	
+			const btSimplePair& pair = m_overlappingPairArray[i];
+			int indexA = pair.m_indexA;
+			int indexB = pair.m_indexB;
+			
+			int	hashValue = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));	// New hash value with new mask
+			m_next[i] = m_hashTable[hashValue];
+			m_hashTable[hashValue] = i;
+		}
+
+
+	}
+}
+
+btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB)
+{
+
+	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));	// New hash value with new mask
+
+
+	btSimplePair* pair = internalFindPair(indexA, indexB, hash);
+	if (pair != NULL)
+	{
+		return pair;
+	}
+
+	int count = m_overlappingPairArray.size();
+	int oldCapacity = m_overlappingPairArray.capacity();
+	void* mem = &m_overlappingPairArray.expandNonInitializing();
+
+	int newCapacity = m_overlappingPairArray.capacity();
+
+	if (oldCapacity < newCapacity)
+	{
+		growTables();
+		//hash with new capacity
+		hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
+	}
+	
+	pair = new (mem) btSimplePair(indexA,indexB);
+
+	pair->m_userPointer = 0;
+	
+	m_next[count] = m_hashTable[hash];
+	m_hashTable[hash] = count;
+
+	return pair;
+}
+
+
+
+void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB)
+{
+	gRemoveSimplePairs++;
+	
+
+	/*if (indexA > indexB) 
+		btSwap(indexA, indexB);*/
+
+	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
+
+	btSimplePair* pair = internalFindPair(indexA, indexB, hash);
+	if (pair == NULL)
+	{
+		return 0;
+	}
+
+	
+	void* userData = pair->m_userPointer;
+
+
+	int pairIndex = int(pair - &m_overlappingPairArray[0]);
+	btAssert(pairIndex < m_overlappingPairArray.size());
+
+	// Remove the pair from the hash table.
+	int index = m_hashTable[hash];
+	btAssert(index != BT_SIMPLE_NULL_PAIR);
+
+	int previous = BT_SIMPLE_NULL_PAIR;
+	while (index != pairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != BT_SIMPLE_NULL_PAIR)
+	{
+		btAssert(m_next[previous] == pairIndex);
+		m_next[previous] = m_next[pairIndex];
+	}
+	else
+	{
+		m_hashTable[hash] = m_next[pairIndex];
+	}
+
+	// We now move the last pair into spot of the
+	// pair being removed. We need to fix the hash
+	// table indices to support the move.
+
+	int lastPairIndex = m_overlappingPairArray.size() - 1;
+
+	// If the removed pair is the last pair, we are done.
+	if (lastPairIndex == pairIndex)
+	{
+		m_overlappingPairArray.pop_back();
+		return userData;
+	}
+
+	// Remove the last pair from the hash table.
+	const btSimplePair* last = &m_overlappingPairArray[lastPairIndex];
+		/* missing swap here too, Nat. */ 
+	int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_indexA), static_cast<unsigned int>(last->m_indexB)) & (m_overlappingPairArray.capacity()-1));
+
+	index = m_hashTable[lastHash];
+	btAssert(index != BT_SIMPLE_NULL_PAIR);
+
+	previous = BT_SIMPLE_NULL_PAIR;
+	while (index != lastPairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != BT_SIMPLE_NULL_PAIR)
+	{
+		btAssert(m_next[previous] == lastPairIndex);
+		m_next[previous] = m_next[lastPairIndex];
+	}
+	else
+	{
+		m_hashTable[lastHash] = m_next[lastPairIndex];
+	}
+
+	// Copy the last pair into the remove pair's spot.
+	m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
+
+	// Insert the last pair into the hash table
+	m_next[pairIndex] = m_hashTable[lastHash];
+	m_hashTable[lastHash] = pairIndex;
+
+	m_overlappingPairArray.pop_back();
+
+	return userData;
+}
+//#include <stdio.h>
+
+
+
+
+
+
+
+
+
+

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h

@@ -0,0 +1,174 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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_HASHED_SIMPLE_PAIR_CACHE_H
+#define BT_HASHED_SIMPLE_PAIR_CACHE_H
+
+
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+const int BT_SIMPLE_NULL_PAIR=0xffffffff;
+
+struct btSimplePair
+{
+	btSimplePair(int indexA,int indexB)
+		:m_indexA(indexA),
+		m_indexB(indexB),
+		m_userPointer(0)
+	{
+	}
+
+	int m_indexA;
+	int m_indexB;
+	union
+	{
+		void*	m_userPointer;
+		int		m_userValue;
+	};
+};
+
+typedef btAlignedObjectArray<btSimplePair>	btSimplePairArray;
+
+
+
+extern int gOverlappingSimplePairs;
+extern int gRemoveSimplePairs;
+extern int gAddedSimplePairs;
+extern int gFindSimplePairs;
+
+
+
+
+class btHashedSimplePairCache
+{
+	btSimplePairArray	m_overlappingPairArray;
+	
+	bool		m_blockedForChanges;
+	
+
+protected:
+	
+	btAlignedObjectArray<int>	m_hashTable;
+	btAlignedObjectArray<int>	m_next;
+	
+
+public:
+	btHashedSimplePairCache();
+	virtual ~btHashedSimplePairCache();
+	
+	void removeAllPairs();
+
+	virtual void*	removeOverlappingPair(int indexA,int indexB);
+	
+	// Add a pair and return the new pair. If the pair already exists,
+	// no new pair is created and the old one is returned.
+	virtual btSimplePair* 	addOverlappingPair(int indexA,int indexB)
+	{
+		gAddedSimplePairs++;
+
+		return internalAddPair(indexA,indexB);
+	}
+
+	
+	virtual btSimplePair*	getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	const btSimplePair*	getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	btSimplePairArray&	getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
+
+	const btSimplePairArray&	getOverlappingPairArray() const
+	{
+		return m_overlappingPairArray;
+	}
+
+	
+	btSimplePair* findPair(int indexA,int indexB);
+
+	int GetCount() const { return m_overlappingPairArray.size(); }
+
+	int	getNumOverlappingPairs() const
+	{
+		return m_overlappingPairArray.size();
+	}
+private:
+	
+	btSimplePair* 	internalAddPair(int indexA, int indexB);
+
+	void	growTables();
+
+	SIMD_FORCE_INLINE bool equalsPair(const btSimplePair& pair, int indexA, int indexB)
+	{	
+		return pair.m_indexA == indexA && pair.m_indexB == indexB;
+	}
+
+	
+	
+	SIMD_FORCE_INLINE	unsigned int getHash(unsigned int indexA, unsigned int indexB)
+	{
+		int key = static_cast<int>(((unsigned int)indexA) | (((unsigned int)indexB) <<16));
+		// Thomas Wang's hash
+
+		key += ~(key << 15);
+		key ^=  (key >> 10);
+		key +=  (key << 3);
+		key ^=  (key >> 6);
+		key += ~(key << 11);
+		key ^=  (key >> 16);
+		return static_cast<unsigned int>(key);
+	}
+	
+
+
+
+
+	SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA , int proxyIdB, int hash)
+	{
+		
+		int index = m_hashTable[hash];
+		
+		while( index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false)
+		{
+			index = m_next[index];
+		}
+
+		if ( index == BT_SIMPLE_NULL_PAIR )
+		{
+			return NULL;
+		}
+
+		btAssert(index < m_overlappingPairArray.size());
+
+		return &m_overlappingPairArray[index];
+	}
+
+	
+};
+
+
+
+
+#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H
+
+

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btCompoundShape.cpp

@@ -273,6 +273,8 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf
 
 
 
+
+
 void btCompoundShape::setLocalScaling(const btVector3& scaling)
 {
 
@@ -283,7 +285,7 @@ void btCompoundShape::setLocalScaling(const btVector3& scaling)
 //		childScale = childScale * (childTrans.getBasis() * scaling);
 		childScale = childScale * scaling / m_localScaling;
 		m_children[i].m_childShape->setLocalScaling(childScale);
-		childTrans.setOrigin((childTrans.getOrigin())*scaling);
+		childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling);
 		updateChildTransform(i, childTrans,false);
 	}
 	

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btConeShape.cpp

@@ -62,6 +62,10 @@ void	btConeShape::setConeUpIndex(int upIndex)
 	default:
 		btAssert(0);
 	};
+	
+	m_implicitShapeDimensions[m_coneIndices[0]] = m_radius;
+	m_implicitShapeDimensions[m_coneIndices[1]] = m_height;
+	m_implicitShapeDimensions[m_coneIndices[2]] = m_radius;
 }
 
 btVector3 btConeShape::coneLocalSupport(const btVector3& v) const

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btConeShape.h

@@ -90,6 +90,13 @@ public:
 	}
 
 	virtual void	setLocalScaling(const btVector3& scaling);
+	
+	
+	virtual	int	calculateSerializeBufferSize() const;
+	
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+	
 
 };
 
@@ -104,19 +111,61 @@ class btConeShapeX : public btConeShape
 		return btVector3 (1,0,0);
 	}
 
+	//debugging
+	virtual const char*	getName()const
+	{
+		return "ConeX";
+	}
+	
+	
 };
 
 ///btConeShapeZ implements a Cone shape, around the Z axis
 class btConeShapeZ : public btConeShape
 {
-	public:
-		btConeShapeZ(btScalar radius,btScalar height);
+public:
+	btConeShapeZ(btScalar radius,btScalar height);
 
 	virtual btVector3	getAnisotropicRollingFrictionDirection() const
 	{
 		return btVector3 (0,0,1);
 	}
 
+	//debugging
+	virtual const char*	getName()const
+	{
+		return "ConeZ";
+	}
+	
+	
 };
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btConeShapeData
+{
+	btConvexInternalShapeData	m_convexInternalShapeData;
+	
+	int	m_upIndex;
+	
+	char	m_padding[4];
+};
+
+SIMD_FORCE_INLINE	int	btConeShape::calculateSerializeBufferSize() const
+{
+	return sizeof(btConeShapeData);
+}
+
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE	const char*	btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btConeShapeData* shapeData = (btConeShapeData*) dataBuffer;
+	
+	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
+	
+	shapeData->m_upIndex = m_coneIndices[1];
+	
+	return "btConeShapeData";
+}
+
 #endif //BT_CONE_MINKOWSKI_H
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btConvexShape.cpp

@@ -21,6 +21,7 @@ subject to the following restrictions:
 #include "btTriangleShape.h"
 #include "btSphereShape.h"
 #include "btCylinderShape.h"
+#include "btConeShape.h"
 #include "btCapsuleShape.h"
 #include "btConvexHullShape.h"
 #include "btConvexPointCloudShape.h"
@@ -336,6 +337,11 @@ btScalar btConvexShape::getMarginNonVirtual () const
 		btCylinderShape* cylShape = (btCylinderShape*)this;
 		return cylShape->getMarginNV();
 	}
+	case CONE_SHAPE_PROXYTYPE:
+	{
+		btConeShape* conShape = (btConeShape*)this;
+		return conShape->getMarginNV();
+	}
 	case CAPSULE_SHAPE_PROXYTYPE:
 	{
 		btCapsuleShape* capsuleShape = (btCapsuleShape*)this;

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

@@ -369,7 +369,7 @@ void	btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
         getVertex(x+1,j+1,vertices[2]);
         callback->processTriangle(vertices,x,j);
         //second triangle
-        getVertex(x,j,vertices[0]);
+      //  getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman
         getVertex(x+1,j+1,vertices[1]);
         getVertex(x,j+1,vertices[2]);
         callback->processTriangle(vertices,x,j);				
@@ -382,7 +382,7 @@ void	btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
         callback->processTriangle(vertices,x,j);
         //second triangle
         getVertex(x+1,j,vertices[0]);
-        getVertex(x,j+1,vertices[1]);
+        //getVertex(x,j+1,vertices[1]);
         getVertex(x+1,j+1,vertices[2]);
         callback->processTriangle(vertices,x,j);
 			}

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp

@@ -111,10 +111,10 @@ int	btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicat
 					return i/3;
 				}
 			}
-	}
-		m_3componentVertices.push_back((float)vertex.getX());
-		m_3componentVertices.push_back((float)vertex.getY());
-		m_3componentVertices.push_back((float)vertex.getZ());
+		}
+		m_3componentVertices.push_back(vertex.getX());
+		m_3componentVertices.push_back(vertex.getY());
+		m_3componentVertices.push_back(vertex.getZ());
 		m_indexedMeshes[0].m_numVertices++;
 		m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0];
 		return (m_3componentVertices.size()/3)-1;

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btTriangleMesh.h

@@ -27,7 +27,7 @@ subject to the following restrictions:
 class btTriangleMesh : public btTriangleIndexVertexArray
 {
 	btAlignedObjectArray<btVector3>	m_4componentVertices;
-	btAlignedObjectArray<float>		m_3componentVertices;
+	btAlignedObjectArray<btScalar>	m_3componentVertices;
 
 	btAlignedObjectArray<unsigned int>		m_32bitIndices;
 	btAlignedObjectArray<unsigned short int>		m_16bitIndices;

Source/ThirdParty/Bullet/src/BulletCollision/Gimpact/btCompoundFromGimpact.h

@@ -0,0 +1,93 @@
+#ifndef BT_COMPOUND_FROM_GIMPACT
+#define BT_COMPOUND_FROM_GIMPACT
+
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "btGImpactShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
+
+struct MyCallback : public btTriangleRaycastCallback
+		{
+			int	m_ignorePart;
+			int	m_ignoreTriangleIndex;
+			
+
+			MyCallback(const btVector3& from, const btVector3& to, int ignorePart, int ignoreTriangleIndex)
+			:btTriangleRaycastCallback(from,to),
+			m_ignorePart(ignorePart),
+			m_ignoreTriangleIndex(ignoreTriangleIndex)
+			{
+				
+			}
+			virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex)
+			{
+				if (partId!=m_ignorePart || triangleIndex!=m_ignoreTriangleIndex)
+				{
+					if (hitFraction < m_hitFraction)
+						return hitFraction;
+				}
+
+				return m_hitFraction;
+			}
+		};
+		struct MyInternalTriangleIndexCallback :public btInternalTriangleIndexCallback
+		{
+			const btGImpactMeshShape*		m_gimpactShape;
+			btCompoundShape*			m_colShape;
+			btScalar	m_depth;
+
+			MyInternalTriangleIndexCallback (btCompoundShape* colShape, const btGImpactMeshShape* meshShape, btScalar depth)
+			:m_colShape(colShape),
+			m_gimpactShape(meshShape),
+			m_depth(depth)
+			{
+			}
+			
+			virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+			{
+				btVector3 scale = m_gimpactShape->getLocalScaling();
+				btVector3 v0=triangle[0]*scale;
+				btVector3 v1=triangle[1]*scale;
+				btVector3 v2=triangle[2]*scale;
+				
+				btVector3 centroid = (v0+v1+v2)/3;
+				btVector3 normal = (v1-v0).cross(v2-v0);
+				normal.normalize();
+				btVector3 rayFrom = centroid;
+				btVector3 rayTo = centroid-normal*m_depth;
+				
+				MyCallback cb(rayFrom,rayTo,partId,triangleIndex);
+				
+				m_gimpactShape->processAllTrianglesRay(&cb,rayFrom, rayTo);
+				if (cb.m_hitFraction<1)
+				{
+					rayTo.setInterpolate3(cb.m_from,cb.m_to,cb.m_hitFraction);
+					//rayTo = cb.m_from;
+					//rayTo = rayTo.lerp(cb.m_to,cb.m_hitFraction);
+					//gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0));
+				}
+				
+
+				
+				btBU_Simplex1to4* tet = new btBU_Simplex1to4(v0,v1,v2,rayTo);
+				btTransform ident;
+				ident.setIdentity();
+				m_colShape->addChildShape(ident,tet);
+			}
+		};
+		
+btCompoundShape*	btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth)
+{
+	btCompoundShape* colShape = new btCompoundShape();
+		
+		btTransform tr;
+		tr.setIdentity();
+		
+		MyInternalTriangleIndexCallback cb(colShape,gimpactMesh, depth);
+		btVector3 aabbMin,aabbMax;
+		gimpactMesh->getAabb(tr,aabbMin,aabbMax);
+		gimpactMesh->getMeshInterface()->InternalProcessAllTriangles(&cb,aabbMin,aabbMax);
+
+	return colShape;	
+}	
+
+#endif //BT_COMPOUND_FROM_GIMPACT

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h

@@ -35,7 +35,7 @@ public:
       kF_None                 = 0,
       kF_FilterBackfaces      = 1 << 0,
       kF_KeepUnflippedNormal  = 1 << 1,   // Prevents returned face normal getting flipped when a ray hits a back-facing triangle
-
+	  kF_UseSubSimplexConvexCastRaytest =  1 << 2,   // Uses an approximate but faster ray versus convex intersection algorithm
       kF_Terminator        = 0xFFFFFFFF
    };
    unsigned int m_flags;

Source/ThirdParty/Bullet/src/BulletDynamics/Character/btCharacterControllerInterface.h

@@ -31,7 +31,7 @@ public:
 	
 	virtual void	setWalkDirection(const btVector3& walkDirection) = 0;
 	virtual void	setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0;
-	virtual void	reset () = 0;
+	virtual void	reset ( btCollisionWorld* collisionWorld ) = 0;
 	virtual void	warp (const btVector3& origin) = 0;
 
 	virtual void	preStep ( btCollisionWorld* collisionWorld) = 0;
@@ -40,6 +40,7 @@ public:
 	virtual void	jump () = 0;
 
 	virtual bool	onGround () const = 0;
+	virtual void	setUpInterpolate (bool value) = 0;
 };
 
 #endif //BT_CHARACTER_CONTROLLER_INTERFACE_H

Source/ThirdParty/Bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp

@@ -14,6 +14,7 @@ subject to the following restrictions:
 */
 
 
+#include <stdio.h>
 #include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/CollisionDispatch/btGhostObject.h"
 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
@@ -77,6 +78,9 @@ public:
 		if (convexResult.m_hitCollisionObject == m_me)
 			return btScalar(1.0);
 
+		if (!convexResult.m_hitCollisionObject->hasContactResponse())
+			return btScalar(1.0);
+
 		btVector3 hitNormalWorld;
 		if (normalInWorldSpace)
 		{
@@ -146,7 +150,11 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho
 	m_jumpSpeed = 10.0; // ?
 	m_wasOnGround = false;
 	m_wasJumping = false;
+	m_interpolateUp = true;
 	setMaxSlope(btRadians(45.0));
+	m_currentStepOffset = 0;
+	full_drop = false;
+	bounce_fix = false;
 }
 
 btKinematicCharacterController::~btKinematicCharacterController ()
@@ -187,6 +195,12 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 		m_manifoldArray.resize(0);
 
 		btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
+
+		btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
+                btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
+
+		if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
+			continue;
 		
 		if (collisionPair->m_algorithm)
 			collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
@@ -260,7 +274,10 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
 		{
 			// we moved up only a fraction of the step height
 			m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
-			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+			if (m_interpolateUp == true)
+				m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+			else
+				m_currentPosition = m_targetPosition;
 		}
 		m_verticalVelocity = 0.0;
 		m_verticalOffset = 0.0;
@@ -325,7 +342,8 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 	{
 		if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
 		{
-			updateTargetPositionBasedOnCollision (m_touchingNormal);
+			//interferes with step movement
+			//updateTargetPositionBasedOnCollision (m_touchingNormal);
 		}
 	}
 
@@ -397,7 +415,8 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 
 void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
 {
-	btTransform start, end;
+	btTransform start, end, end_double;
+	bool runonce = false;
 
 	// phase 3: down
 	/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
@@ -406,44 +425,124 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 	btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; 
 	m_targetPosition -= (step_drop + gravity_drop);*/
 
+	btVector3 orig_position = m_targetPosition;
+	
 	btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
-	if(downVelocity > 0.0 && downVelocity < m_stepHeight
+
+	if(downVelocity > 0.0 && downVelocity > m_fallSpeed
 		&& (m_wasOnGround || !m_wasJumping))
-	{
-		downVelocity = m_stepHeight;
-	}
+		downVelocity = m_fallSpeed;
 
 	btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
 	m_targetPosition -= step_drop;
 
-	start.setIdentity ();
-	end.setIdentity ();
+	btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
+        callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+        callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
-	start.setOrigin (m_currentPosition);
-	end.setOrigin (m_targetPosition);
+        btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
+        callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+        callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
-	btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
-	callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
-	callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
-	
-	if (m_useGhostObjectSweepTest)
-	{
-		m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-	} else
+	while (1)
 	{
-		collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+		start.setIdentity ();
+		end.setIdentity ();
+
+		end_double.setIdentity ();
+
+		start.setOrigin (m_currentPosition);
+		end.setOrigin (m_targetPosition);
+
+		//set double test for 2x the step drop, to check for a large drop vs small drop
+		end_double.setOrigin (m_targetPosition - step_drop);
+
+		if (m_useGhostObjectSweepTest)
+		{
+			m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+
+			if (!callback.hasHit())
+			{
+				//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
+				m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			}
+		} else
+		{
+			collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+
+			if (!callback.hasHit())
+					{
+							//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
+							collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+					}
+		}
+	
+		btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+		bool has_hit = false;
+		if (bounce_fix == true)
+			has_hit = callback.hasHit() || callback2.hasHit();
+		else
+			has_hit = callback2.hasHit();
+
+		if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
+					&& (m_wasOnGround || !m_wasJumping))
+		{
+			//redo the velocity calculation when falling a small amount, for fast stairs motion
+			//for larger falls, use the smoother/slower interpolated movement by not touching the target position
+
+			m_targetPosition = orig_position;
+					downVelocity = m_stepHeight;
+
+				btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+			m_targetPosition -= step_drop;
+			runonce = true;
+			continue; //re-run previous tests
+		}
+		break;
 	}
 
-	if (callback.hasHit())
+	if (callback.hasHit() || runonce == true)
 	{
 		// we dropped a fraction of the height -> hit floor
-		m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+
+		btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
+
+		//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
+
+		if (bounce_fix == true)
+		{
+			if (full_drop == true)
+                                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+                        else
+                                //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
+                                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
+		}
+		else
+			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+
+		full_drop = false;
+
 		m_verticalVelocity = 0.0;
 		m_verticalOffset = 0.0;
 		m_wasJumping = false;
 	} else {
 		// we dropped the full height
 		
+		full_drop = true;
+
+		if (bounce_fix == true)
+		{
+			downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+			if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
+			{
+				m_targetPosition += step_drop; //undo previous target change
+				downVelocity = m_fallSpeed;
+				step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+				m_targetPosition -= step_drop;
+			}
+		}
+		//printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY());
+
 		m_currentPosition = m_targetPosition;
 	}
 }
@@ -476,13 +575,24 @@ btScalar timeInterval
 	m_useWalkDirection = false;
 	m_walkDirection = velocity;
 	m_normalizedDirection = getNormalizedVector(m_walkDirection);
-	m_velocityTimeInterval = timeInterval;
+	m_velocityTimeInterval += timeInterval;
 }
 
-
-
-void btKinematicCharacterController::reset ()
-{
+void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
+{
+        m_verticalVelocity = 0.0;
+        m_verticalOffset = 0.0;
+        m_wasOnGround = false;
+        m_wasJumping = false;
+        m_walkDirection.setValue(0,0,0);
+        m_velocityTimeInterval = 0.0;
+
+        //clear pair cache
+        btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
+        while (cache->getOverlappingPairArray().size() > 0)
+        {
+                cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
+        }
 }
 
 void btKinematicCharacterController::warp (const btVector3& origin)
@@ -653,3 +763,8 @@ btVector3* btKinematicCharacterController::getUpAxisDirections()
 void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
 {
 }
+
+void btKinematicCharacterController::setUpInterpolate(bool value)
+{
+	m_interpolateUp = value;
+}

Source/ThirdParty/Bullet/src/BulletDynamics/Character/btKinematicCharacterController.h

@@ -81,6 +81,9 @@ protected:
 	int m_upAxis;
 
 	static btVector3* getUpAxisDirections();
+	bool  m_interpolateUp;
+	bool  full_drop;
+	bool  bounce_fix;
 
 	btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
 	btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
@@ -133,7 +136,7 @@ public:
 	virtual void setVelocityForTimeInterval(const btVector3& velocity,
 				btScalar timeInterval);
 
-	void reset ();
+	void reset ( btCollisionWorld* collisionWorld );
 	void warp (const btVector3& origin);
 
 	void preStep (  btCollisionWorld* collisionWorld);
@@ -161,6 +164,7 @@ public:
 	}
 
 	bool onGround () const;
+	void setUpInterpolate (bool value);
 };
 
 #endif // BT_KINEMATIC_CHARACTER_CONTROLLER_H

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

@@ -40,6 +40,15 @@ and swing 1 and 2 are along the z and y axes respectively.
 #include "btJacobianEntry.h"
 #include "btTypedConstraint.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btConeTwistConstraintData2	btConeTwistConstraintDoubleData
+#define btConeTwistConstraintDataName	"btConeTwistConstraintDoubleData"
+#else
+#define btConeTwistConstraintData2	btConeTwistConstraintData 
+#define btConeTwistConstraintDataName	"btConeTwistConstraintData" 
+#endif //BT_USE_DOUBLE_PRECISION
+
+
 class btRigidBody;
 
 enum btConeTwistFlags
@@ -295,7 +304,30 @@ public:
 
 };
 
-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+
+	
+struct	btConeTwistConstraintDoubleData
+{
+	btTypedConstraintDoubleData	m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame;
+	btTransformDoubleData m_rbBFrame;
+
+	//limits
+	double	m_swingSpan1;
+	double	m_swingSpan2;
+	double	m_twistSpan;
+	double	m_limitSoftness;
+	double	m_biasFactor;
+	double	m_relaxationFactor;
+
+	double	m_damping;
+		
+	
+
+};
+
+#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+///this structure is not used, except for loading pre-2.82 .bullet files
 struct	btConeTwistConstraintData
 {
 	btTypedConstraintData	m_typeConstraintData;
@@ -315,12 +347,12 @@ struct	btConeTwistConstraintData
 	char m_pad[4];
 
 };
-	
-
+#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+//
 
 SIMD_FORCE_INLINE int	btConeTwistConstraint::calculateSerializeBufferSize() const
 {
-	return sizeof(btConeTwistConstraintData);
+	return sizeof(btConeTwistConstraintData2);
 
 }
 
@@ -328,21 +360,21 @@ SIMD_FORCE_INLINE int	btConeTwistConstraint::calculateSerializeBufferSize() cons
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
 SIMD_FORCE_INLINE const char*	btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer;
+	btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*) dataBuffer;
 	btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
 
-	m_rbAFrame.serializeFloat(cone->m_rbAFrame);
-	m_rbBFrame.serializeFloat(cone->m_rbBFrame);
+	m_rbAFrame.serialize(cone->m_rbAFrame);
+	m_rbBFrame.serialize(cone->m_rbBFrame);
 	
-	cone->m_swingSpan1 = float(m_swingSpan1);
-	cone->m_swingSpan2 = float(m_swingSpan2);
-	cone->m_twistSpan = float(m_twistSpan);
-	cone->m_limitSoftness = float(m_limitSoftness);
-	cone->m_biasFactor = float(m_biasFactor);
-	cone->m_relaxationFactor = float(m_relaxationFactor);
-	cone->m_damping = float(m_damping);
-
-	return "btConeTwistConstraintData";
+	cone->m_swingSpan1 = m_swingSpan1;
+	cone->m_swingSpan2 = m_swingSpan2;
+	cone->m_twistSpan = m_twistSpan;
+	cone->m_limitSoftness = m_limitSoftness;
+	cone->m_biasFactor = m_biasFactor;
+	cone->m_relaxationFactor = m_relaxationFactor;
+	cone->m_damping = m_damping;
+
+	return btConeTwistConstraintDataName;
 }
 
 

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h

@@ -28,6 +28,14 @@ class btIDebugDraw;
 class btStackAlloc;
 class	btDispatcher;
 /// btConstraintSolver provides solver interface
+
+
+enum btConstraintSolverType
+{
+	BT_SEQUENTIAL_IMPULSE_SOLVER=1,
+	BT_MLCP_SOLVER=2
+};
+
 class btConstraintSolver
 {
 
@@ -44,6 +52,10 @@ public:
 
 	///clear internal cached data and reset random seed
 	virtual	void	reset() = 0;
+
+	virtual btConstraintSolverType	getSolverType() const=0;
+
+
 };
 
 

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp

@@ -0,0 +1,129 @@
+/*
+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 "btFixedConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include <new>
+
+
+btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
+:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
+{
+	m_pivotInA = frameInA.getOrigin();
+	m_pivotInB = frameInB.getOrigin();
+	m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
+
+}
+
+btFixedConstraint::~btFixedConstraint ()
+{
+}
+
+	
+void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	info->m_numConstraintRows = 6;
+	info->nub = 6;
+}
+
+void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	//fix the 3 linear degrees of freedom
+
+	
+	const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
+	const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
+	const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
+	const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis();
+	
+
+	info->m_J1linearAxis[0] = 1;
+	info->m_J1linearAxis[info->rowskip+1] = 1;
+	info->m_J1linearAxis[2*info->rowskip+2] = 1;
+
+	btVector3 a1 = worldOrnA*m_pivotInA;
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+		btVector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+    
+	if (info->m_J2linearAxis)
+	{
+		info->m_J2linearAxis[0] = -1;
+		info->m_J2linearAxis[info->rowskip+1] = -1;
+		info->m_J2linearAxis[2*info->rowskip+2] = -1;
+	}
+	
+	btVector3 a2 = worldOrnB*m_pivotInB;
+   
+	{
+	//	btVector3 a2n = -a2;
+		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+
+    // set right hand side for the linear dofs
+	btScalar k = info->fps * info->erp;
+	
+	btVector3 linearError = k*(a2+worldPosB-a1-worldPosA);
+    int j;
+	for (j=0; j<3; j++)
+    {
+
+
+
+        info->m_constraintError[j*info->rowskip] = linearError[j];
+		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+    }
+
+		//fix the 3 angular degrees of freedom
+
+	int start_row = 3;
+	int s = info->rowskip;
+    int start_index = start_row * s;
+
+    // 3 rows to make body rotations equal
+	info->m_J1angularAxis[start_index] = 1;
+    info->m_J1angularAxis[start_index + s + 1] = 1;
+    info->m_J1angularAxis[start_index + s*2+2] = 1;
+    if ( info->m_J2angularAxis)
+    {
+        info->m_J2angularAxis[start_index] = -1;
+        info->m_J2angularAxis[start_index + s+1] = -1;
+        info->m_J2angularAxis[start_index + s*2+2] = -1;
+    }
+
+    // set right hand side for the angular dofs
+
+	btVector3 diff;
+	btScalar angle;
+	btMatrix3x3 mrelCur = worldOrnA *worldOrnB.inverse();
+	btQuaternion qrelCur;
+	mrelCur.getRotation(qrelCur);
+	btTransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
+	diff*=-angle;
+	for (j=0; j<3; j++)
+    {
+        info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
+    }
+
+}

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h

@@ -0,0 +1,49 @@
+/*
+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_FIXED_CONSTRAINT_H
+#define BT_FIXED_CONSTRAINT_H
+
+#include "btTypedConstraint.h"
+
+ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
+{
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
+	btQuaternion m_relTargetAB;
+
+public:
+	btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
+	
+	virtual ~btFixedConstraint();
+
+	
+	virtual void getInfo1 (btConstraintInfo1* info);
+
+	virtual void getInfo2 (btConstraintInfo2* info);
+
+	virtual	void	setParam(int num, btScalar value, int axis = -1)
+	{
+		btAssert(0);
+	}
+	virtual	btScalar getParam(int num, int axis = -1) const
+	{
+		btAssert(0);
+		return 0.f;
+	}
+
+};
+
+#endif //BT_FIXED_CONSTRAINT_H

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.h

@@ -19,6 +19,18 @@ subject to the following restrictions:
 #define BT_GEAR_CONSTRAINT_H
 
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btGearConstraintData	btGearConstraintDoubleData
+#define btGearConstraintDataName	"btGearConstraintDoubleData"
+#else
+#define btGearConstraintData	btGearConstraintFloatData
+#define btGearConstraintDataName	"btGearConstraintFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+
+
+
 ///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio.
 ///See Bullet/Demos/ConstraintDemo for an example use.
 class btGearConstraint : public btTypedConstraint
@@ -36,21 +48,105 @@ public:
 	///internal method used by the constraint solver, don't use them directly
 	virtual void getInfo1 (btConstraintInfo1* info);
 
-	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo2 (btConstraintInfo2* info);
-
-	virtual	void	setParam(int num, btScalar value, int axis = -1) 
+	///internal method used by the constraint solver, don't use them directly
+	virtual void getInfo2 (btConstraintInfo2* info);
+
+	void setAxisA(btVector3& axisA) 
 	{
-		btAssert(0);
-	};
+		m_axisInA = axisA;
+	}
+	void setAxisB(btVector3& axisB)
+	{
+		m_axisInB = axisB;
+	}
+	void setRatio(btScalar ratio)
+	{
+		m_ratio = ratio;
+	}
+	const btVector3& getAxisA() const
+	{
+		return m_axisInA;
+	}
+	const btVector3& getAxisB() const
+	{
+		return m_axisInB;
+	}
+	btScalar getRatio() const
+	{
+		return m_ratio;
+	}
+
+
+	virtual	void	setParam(int num, btScalar value, int axis = -1) 
+	{
+		(void) num;
+		(void) value;
+		(void) axis;
+		btAssert(0);
+	}
 
 	///return the local value of parameter
 	virtual	btScalar getParam(int num, int axis = -1) const 
 	{ 
+		(void) num;
+		(void) axis;
 		btAssert(0);
 		return 0.f;
 	}
 
+	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
+
+
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btGearConstraintFloatData
+{
+	btTypedConstraintFloatData	m_typeConstraintData;
+
+	btVector3FloatData			m_axisInA;
+	btVector3FloatData			m_axisInB;
+
+	float							m_ratio;
+	char							m_padding[4];
+};
+
+struct btGearConstraintDoubleData
+{
+	btTypedConstraintDoubleData	m_typeConstraintData;
+
+	btVector3DoubleData			m_axisInA;
+	btVector3DoubleData			m_axisInB;
+
+	double						m_ratio;
+};
+
+SIMD_FORCE_INLINE	int	btGearConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btGearConstraintData);
+}
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE	const char*	btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btGearConstraintData* gear = (btGearConstraintData*)dataBuffer;
+	btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer);
+
+	m_axisInA.serialize( gear->m_axisInA );
+	m_axisInB.serialize( gear->m_axisInB );
+
+	gear->m_ratio = m_ratio;
+
+	return btGearConstraintDataName;
+}
+
+
+
+
+
+
 #endif //BT_GEAR_CONSTRAINT_H

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

@@ -35,6 +35,14 @@ class btRigidBody;
 
 
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btGeneric6DofConstraintData2		btGeneric6DofConstraintDoubleData2
+#define btGeneric6DofConstraintDataName	"btGeneric6DofConstraintDoubleData2"
+#else
+#define btGeneric6DofConstraintData2		btGeneric6DofConstraintData
+#define btGeneric6DofConstraintDataName	"btGeneric6DofConstraintData"
+#endif //BT_USE_DOUBLE_PRECISION
+
 
 //! Rotation Limit structure for generic joints
 class btRotationalLimitMotor
@@ -561,7 +569,7 @@ public:
 	
 };
 
-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+
 struct btGeneric6DofConstraintData
 {
 	btTypedConstraintData	m_typeConstraintData;
@@ -578,35 +586,51 @@ struct btGeneric6DofConstraintData
 	int m_useOffsetForConstraintFrame;
 };
 
+struct btGeneric6DofConstraintDoubleData2
+{
+	btTypedConstraintDoubleData	m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTransformDoubleData m_rbBFrame;
+	
+	btVector3DoubleData	m_linearUpperLimit;
+	btVector3DoubleData	m_linearLowerLimit;
+
+	btVector3DoubleData	m_angularUpperLimit;
+	btVector3DoubleData	m_angularLowerLimit;
+	
+	int	m_useLinearReferenceFrameA;
+	int m_useOffsetForConstraintFrame;
+};
+
 SIMD_FORCE_INLINE	int	btGeneric6DofConstraint::calculateSerializeBufferSize() const
 {
-	return sizeof(btGeneric6DofConstraintData);
+	return sizeof(btGeneric6DofConstraintData2);
 }
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
 SIMD_FORCE_INLINE	const char*	btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 
-	btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer;
+	btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer;
 	btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
 
-	m_frameInA.serializeFloat(dof->m_rbAFrame);
-	m_frameInB.serializeFloat(dof->m_rbBFrame);
+	m_frameInA.serialize(dof->m_rbAFrame);
+	m_frameInB.serialize(dof->m_rbBFrame);
 
 		
 	int i;
 	for (i=0;i<3;i++)
 	{
-		dof->m_angularLowerLimit.m_floats[i] =  float(m_angularLimits[i].m_loLimit);
-		dof->m_angularUpperLimit.m_floats[i] =  float(m_angularLimits[i].m_hiLimit);
-		dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]);
-		dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]);
+		dof->m_angularLowerLimit.m_floats[i] =  m_angularLimits[i].m_loLimit;
+		dof->m_angularUpperLimit.m_floats[i] =  m_angularLimits[i].m_hiLimit;
+		dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i];
+		dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i];
 	}
 	
 	dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
 	dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
 
-	return "btGeneric6DofConstraintData";
+	return btGeneric6DofConstraintDataName;
 }
 
 

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h

@@ -21,6 +21,15 @@ subject to the following restrictions:
 #include "btTypedConstraint.h"
 #include "btGeneric6DofConstraint.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btGeneric6DofSpringConstraintData2		btGeneric6DofSpringConstraintDoubleData2
+#define btGeneric6DofSpringConstraintDataName	"btGeneric6DofSpringConstraintDoubleData2"
+#else
+#define btGeneric6DofSpringConstraintData2		btGeneric6DofSpringConstraintData
+#define btGeneric6DofSpringConstraintDataName	"btGeneric6DofSpringConstraintData"
+#endif //BT_USE_DOUBLE_PRECISION
+
+
 
 /// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF
 
@@ -65,7 +74,6 @@ public:
 };
 
 
-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct btGeneric6DofSpringConstraintData
 {
 	btGeneric6DofConstraintData	m_6dofData;
@@ -76,26 +84,37 @@ struct btGeneric6DofSpringConstraintData
 	float		m_springDamping[6];
 };
 
+struct btGeneric6DofSpringConstraintDoubleData2
+{
+	btGeneric6DofConstraintDoubleData2	m_6dofData;
+	
+	int			m_springEnabled[6];
+	double		m_equilibriumPoint[6];
+	double		m_springStiffness[6];
+	double		m_springDamping[6];
+};
+
+
 SIMD_FORCE_INLINE	int	btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const
 {
-	return sizeof(btGeneric6DofSpringConstraintData);
+	return sizeof(btGeneric6DofSpringConstraintData2);
 }
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
 SIMD_FORCE_INLINE	const char*	btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btGeneric6DofSpringConstraintData* dof = (btGeneric6DofSpringConstraintData*)dataBuffer;
+	btGeneric6DofSpringConstraintData2* dof = (btGeneric6DofSpringConstraintData2*)dataBuffer;
 	btGeneric6DofConstraint::serialize(&dof->m_6dofData,serializer);
 
 	int i;
 	for (i=0;i<6;i++)
 	{
-		dof->m_equilibriumPoint[i] = (float)m_equilibriumPoint[i];
-		dof->m_springDamping[i] = (float)m_springDamping[i];
+		dof->m_equilibriumPoint[i] = m_equilibriumPoint[i];
+		dof->m_springDamping[i] = m_springDamping[i];
 		dof->m_springEnabled[i] = m_springEnabled[i]? 1 : 0;
-		dof->m_springStiffness[i] = (float)m_springStiffness[i];
+		dof->m_springStiffness[i] = m_springStiffness[i];
 	}
-	return "btGeneric6DofSpringConstraintData";
+	return btGeneric6DofSpringConstraintDataName;
 }
 
 #endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -28,8 +28,8 @@ subject to the following restrictions:
 class btRigidBody;
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btHingeConstraintData	btHingeConstraintDoubleData
-#define btHingeConstraintDataName	"btHingeConstraintDoubleData"
+#define btHingeConstraintData	btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version
+#define btHingeConstraintDataName	"btHingeConstraintDoubleData2" 
 #else
 #define btHingeConstraintData	btHingeConstraintFloatData
 #define btHingeConstraintDataName	"btHingeConstraintFloatData"
@@ -302,7 +302,10 @@ public:
 
 };
 
-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+
+//only for backward compatibility
+#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+///this structure is not used, except for loading pre-2.82 .bullet files
 struct	btHingeConstraintDoubleData
 {
 	btTypedConstraintData	m_typeConstraintData;
@@ -321,7 +324,9 @@ struct	btHingeConstraintDoubleData
 	float	m_relaxationFactor;
 
 };
-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+
+
 struct	btHingeConstraintFloatData
 {
 	btTypedConstraintData	m_typeConstraintData;
@@ -344,6 +349,30 @@ struct	btHingeConstraintFloatData
 
 
 
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btHingeConstraintDoubleData2
+{
+	btTypedConstraintDoubleData	m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTransformDoubleData m_rbBFrame;
+	int			m_useReferenceFrameA;
+	int			m_angularOnly;
+	int			m_enableAngularMotor;
+	double		m_motorTargetVelocity;
+	double		m_maxMotorImpulse;
+
+	double		m_lowerLimit;
+	double		m_upperLimit;
+	double		m_limitSoftness;
+	double		m_biasFactor;
+	double		m_relaxationFactor;
+	char	m_padding1[4];
+
+};
+
+
+
+
 SIMD_FORCE_INLINE	int	btHingeConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btHingeConstraintData);

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h

@@ -24,10 +24,10 @@ class btRigidBody;
 
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btPoint2PointConstraintData	btPoint2PointConstraintDoubleData
-#define btPoint2PointConstraintDataName	"btPoint2PointConstraintDoubleData"
+#define btPoint2PointConstraintData2	btPoint2PointConstraintDoubleData2
+#define btPoint2PointConstraintDataName	"btPoint2PointConstraintDoubleData2"
 #else
-#define btPoint2PointConstraintData	btPoint2PointConstraintFloatData
+#define btPoint2PointConstraintData2	btPoint2PointConstraintFloatData
 #define btPoint2PointConstraintDataName	"btPoint2PointConstraintFloatData"
 #endif //BT_USE_DOUBLE_PRECISION
 
@@ -133,6 +133,17 @@ struct	btPoint2PointConstraintFloatData
 	btVector3FloatData	m_pivotInB;
 };
 
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btPoint2PointConstraintDoubleData2
+{
+	btTypedConstraintDoubleData	m_typeConstraintData;
+	btVector3DoubleData	m_pivotInA;
+	btVector3DoubleData	m_pivotInB;
+};
+
+#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+///this structure is not used, except for loading pre-2.82 .bullet files
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btPoint2PointConstraintDoubleData
 {
@@ -140,18 +151,19 @@ struct	btPoint2PointConstraintDoubleData
 	btVector3DoubleData	m_pivotInA;
 	btVector3DoubleData	m_pivotInB;
 };
+#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 
 
 SIMD_FORCE_INLINE	int	btPoint2PointConstraint::calculateSerializeBufferSize() const
 {
-	return sizeof(btPoint2PointConstraintData);
+	return sizeof(btPoint2PointConstraintData2);
 
 }
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
 SIMD_FORCE_INLINE	const char*	btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer;
+	btPoint2PointConstraintData2* p2pData = (btPoint2PointConstraintData2*)dataBuffer;
 
 	btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
 	m_pivotInA.serialize(p2pData->m_pivotInA);

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -154,7 +154,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 #endif
 }
 
-// Project Gauss Seidel or the equivalent Sequential Impulse
+// Projected Gauss Seidel or the equivalent Sequential Impulse
  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
@@ -282,7 +282,7 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
 
 
 
-void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
+void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep)
 {
 
 	btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
@@ -301,8 +301,8 @@ void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
 		solverBody->m_linearFactor = rb->getLinearFactor();
 		solverBody->m_linearVelocity = rb->getLinearVelocity();
 		solverBody->m_angularVelocity = rb->getAngularVelocity();
-		solverBody->m_externalForce = rb->getTotalForce();
-		solverBody->m_externalTorque = rb->getTotalTorque();
+		solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep;
+		solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;
 		
 	} else
 	{
@@ -313,8 +313,8 @@ void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
 		solverBody->m_linearFactor.setValue(1,1,1);
 		solverBody->m_linearVelocity.setValue(0,0,0);
 		solverBody->m_angularVelocity.setValue(0,0,0);
-		solverBody->m_externalForce.setValue(0,0,0);
-		solverBody->m_externalTorque.setValue(0,0,0);
+		solverBody->m_externalForceImpulse.setValue(0,0,0);
+		solverBody->m_externalTorqueImpulse.setValue(0,0,0);
 	}
 
 
@@ -333,8 +333,7 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel,
 
 
 
-static void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
-static void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
+void	btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
 {
 	
 
@@ -358,8 +357,6 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 {
 
 	
-	solverConstraint.m_contactNormal1 = normalAxis;
-	solverConstraint.m_contactNormal2 = -normalAxis;
 	btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
 	btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
 
@@ -375,15 +372,30 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 	solverConstraint.m_appliedImpulse = 0.f;
 	solverConstraint.m_appliedPushImpulse = 0.f;
 
+	if (body0)
 	{
+		solverConstraint.m_contactNormal1 = normalAxis;
 		btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
 		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor();
+	}else
+	{
+		solverConstraint.m_contactNormal1.setZero();
+		solverConstraint.m_relpos1CrossNormal.setZero();
+		solverConstraint.m_angularComponentA .setZero();
 	}
+
+	if (body1)
 	{
+		solverConstraint.m_contactNormal2 = -normalAxis;
 		btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
 		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor();
+	} else
+	{
+		solverConstraint.m_contactNormal2.setZero();
+		solverConstraint.m_relpos2CrossNormal.setZero();
+		solverConstraint.m_angularComponentB.setZero();
 	}
 
 	{
@@ -408,10 +420,10 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 		
 
 		btScalar rel_vel;
-		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) 
-			+ solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
-		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) 
-			+ solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
+		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) 
+			+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
+		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) 
+			+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
 
 		rel_vel = vel1Dotn+vel2Dotn;
 
@@ -421,8 +433,8 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 		btSimdScalar	velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
 		solverConstraint.m_rhs = velocityImpulse;
 		solverConstraint.m_cfm = cfmSlip;
-		solverConstraint.m_lowerLimit = 0;
-		solverConstraint.m_upperLimit = 1e10f;
+		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
+		solverConstraint.m_upperLimit = solverConstraint.m_friction;
 		
 	}
 }
@@ -488,10 +500,10 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolv
 		
 
 		btScalar rel_vel;
-		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) 
-			+ solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
-		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) 
-			+ solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
+		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) 
+			+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
+		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) 
+			+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
 
 		rel_vel = vel1Dotn+vel2Dotn;
 
@@ -501,8 +513,8 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolv
 		btSimdScalar	velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
 		solverConstraint.m_rhs = velocityImpulse;
 		solverConstraint.m_cfm = cfmSlip;
-		solverConstraint.m_lowerLimit = 0;
-		solverConstraint.m_upperLimit = 1e10f;
+		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
+		solverConstraint.m_upperLimit = solverConstraint.m_friction;
 		
 	}
 }
@@ -524,7 +536,7 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addRollingFrictionConst
 }
 
 
-int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
+int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep)
 {
 
 	int solverBodyIdA = -1;
@@ -542,11 +554,19 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 		{
 			solverBodyIdA = m_tmpSolverBodyPool.size();
 			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-			initSolverBody(&solverBody,&body);
+			initSolverBody(&solverBody,&body,timeStep);
 			body.setCompanionId(solverBodyIdA);
 		} else
 		{
-			return 0;//assume first one is a fixed solver body
+			
+			if (m_fixedBodyId<0)
+			{
+				m_fixedBodyId = m_tmpSolverBodyPool.size();
+				btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+				initSolverBody(&fixedBody,0,timeStep);
+			}
+			return m_fixedBodyId;
+//			return 0;//assume first one is a fixed solver body
 		}
 	}
 
@@ -559,8 +579,8 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, 
 																 int solverBodyIdA, int solverBodyIdB,
 																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
-																 btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
-																 btVector3& rel_pos1, btVector3& rel_pos2)
+																 btScalar& relaxation,
+																 const btVector3& rel_pos1, const btVector3& rel_pos2)
 {
 			
 			const btVector3& pos1 = cp.getPositionWorldOnA();
@@ -574,8 +594,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 
 //			btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
 //			btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
-			rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
-			rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
+			//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
+			//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 
 			relaxation = 1.f;
 
@@ -608,10 +628,24 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 					solverConstraint.m_jacDiagABInv = denom;
 				}
 
-				solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
-				solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
-				solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-				solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+				if (rb0)
+				{
+					solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
+					solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+				} else
+				{
+					solverConstraint.m_contactNormal1.setZero();
+					solverConstraint.m_relpos1CrossNormal.setZero();
+				}
+				if (rb1)
+				{
+					solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
+					solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+				}else
+				{
+					solverConstraint.m_contactNormal2.setZero();
+					solverConstraint.m_relpos2CrossNormal.setZero();
+				}
 
 				btScalar restitution = 0.f;
 				btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
@@ -623,8 +657,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 					vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
 
 	//			btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
-					vel  = vel1 - vel2;
-					rel_vel = cp.m_normalWorldOnB.dot(vel);
+					btVector3 vel  = vel1 - vel2;
+					btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
 
 					
 
@@ -656,10 +690,10 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 
 				{
 
-					btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForce*bodyA->m_invMass*infoGlobal.m_timeStep : btVector3(0,0,0);
-					btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorque*rb0->getInvInertiaTensorWorld()*infoGlobal.m_timeStep : btVector3(0,0,0);
-					btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForce*bodyB->m_invMass*infoGlobal.m_timeStep : btVector3(0,0,0);
-					btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorque*rb1->getInvInertiaTensorWorld()*infoGlobal.m_timeStep : btVector3(0,0,0);
+					btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0);
+					btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0);
+					btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0);
+					btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0);
 						
 
 					btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA) 
@@ -768,8 +802,8 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 	colObj0 = (btCollisionObject*)manifold->getBody0();
 	colObj1 = (btCollisionObject*)manifold->getBody1();
 
-	int solverBodyIdA = getOrInitSolverBody(*colObj0);
-	int solverBodyIdB = getOrInitSolverBody(*colObj1);
+	int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
+	int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
 
 //	btRigidBody* bodyA = btRigidBody::upcast(colObj0);
 //	btRigidBody* bodyB = btRigidBody::upcast(colObj1);
@@ -794,8 +828,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			btVector3 rel_pos1;
 			btVector3 rel_pos2;
 			btScalar relaxation;
-			btScalar rel_vel;
-			btVector3 vel;
+			
 
 			int frictionIndex = m_tmpSolverContactConstraintPool.size();
 			btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
@@ -806,7 +839,24 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 			solverConstraint.m_originalContactPoint = &cp;
 
-			setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
+			const btVector3& pos1 = cp.getPositionWorldOnA();
+			const btVector3& pos2 = cp.getPositionWorldOnB();
+
+			rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
+			rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
+
+			btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+			btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+
+			solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
+			solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
+			
+			btVector3 vel  = vel1 - vel2;
+			btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
+
+			setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
+
+			
 
 //			const btVector3& pos1 = cp.getPositionWorldOnA();
 //			const btVector3& pos2 = cp.getPositionWorldOnB();
@@ -873,6 +923,10 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 				if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
 				{
 					cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
+					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+
 					if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
@@ -880,17 +934,16 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 						addFrictionConstraint(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);
-					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
 				} else
 				{
 					btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
 
+					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
@@ -898,9 +951,6 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 						addFrictionConstraint(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);
-					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
 					{
@@ -915,8 +965,8 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
 
-				setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
 			}
+			setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
 		
 
 			
@@ -925,8 +975,23 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 	}
 }
 
+void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
+{
+	int i;
+	btPersistentManifold* manifold = 0;
+//			btCollisionObject* colObj0=0,*colObj1=0;
+
+
+	for (i=0;i<numManifolds;i++)
+	{
+		manifold = manifoldPtr[i];
+		convertContact(manifold,infoGlobal);
+	}
+}
+
 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
 {
+	m_fixedBodyId = -1;
 	BT_PROFILE("solveGroupCacheFriendlySetup");
 	(void)debugDrawer;
 
@@ -1011,14 +1076,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 	m_tmpSolverBodyPool.reserve(numBodies+1);
 	m_tmpSolverBodyPool.resize(0);
 
-	btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
-    initSolverBody(&fixedBody,0);
+	//btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+    //initSolverBody(&fixedBody,0);
 
 	//convert all bodies
 
 	for (int i=0;i<numBodies;i++)
 	{
-		int bodyId = getOrInitSolverBody(*bodies[i]);
+		int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
 
 		btRigidBody* body = btRigidBody::upcast(bodies[i]);
 		if (body && body->getInvMass())
@@ -1028,7 +1093,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 			if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE)
 			{
 				gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
-				solverBody.m_externalTorque -= gyroForce;
+				solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
 			}
 		}
 	}
@@ -1099,8 +1164,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 					btRigidBody& rbA = constraint->getRigidBodyA();
 					btRigidBody& rbB = constraint->getRigidBodyB();
 
-                    int solverBodyIdA = getOrInitSolverBody(rbA);
-                    int solverBodyIdB = getOrInitSolverBody(rbB);
+					int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep);
+                    int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep);
 
                     btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
                     btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
@@ -1200,11 +1265,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 						
 						{
 							btScalar rel_vel;
-							btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForce*bodyAPtr->m_invMass*infoGlobal.m_timeStep : btVector3(0,0,0);
-							btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorque*rbA.getInvInertiaTensorWorld()*infoGlobal.m_timeStep : btVector3(0,0,0);
+							btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
+							btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
 
-							btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForce*bodyBPtr->m_invMass*infoGlobal.m_timeStep : btVector3(0,0,0);
-							btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorque*rbB.getInvInertiaTensorWorld()*infoGlobal.m_timeStep : btVector3(0,0,0);
+							btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
+							btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
 							
 							btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) 
 												+ solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
@@ -1229,18 +1294,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 			}
 		}
 
-		{
-			int i;
-			btPersistentManifold* manifold = 0;
-//			btCollisionObject* colObj0=0,*colObj1=0;
-
+		convertContacts(manifoldPtr,numManifolds,infoGlobal);
 
-			for (i=0;i<numManifolds;i++)
-			{
-				manifold = manifoldPtr[i];
-				convertContact(manifold,infoGlobal);
-			}
-		}
 	}
 
 //	btContactSolverInfo info = infoGlobal;
@@ -1334,8 +1389,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 			{
 				if (constraints[j]->isEnabled())
 				{
-					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
-					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
+					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
+					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
 					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
 					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
 					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
@@ -1399,7 +1454,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 				for (j=0;j<numPoolConstraints;j++)
 				{
 					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 
 				}
 		
@@ -1418,7 +1474,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
 						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
 
-						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 					}
 				}
 
@@ -1462,8 +1519,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 			{
 				if (constraints[j]->isEnabled())
 				{
-					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
-					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
+					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
+					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
 					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
 					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
 					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
@@ -1639,11 +1696,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 			
 			m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
 				m_tmpSolverBodyPool[i].m_linearVelocity+
-				m_tmpSolverBodyPool[i].m_externalForce*body->getInvMass()*infoGlobal.m_timeStep);
+				m_tmpSolverBodyPool[i].m_externalForceImpulse);
 
 			m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
 				m_tmpSolverBodyPool[i].m_angularVelocity+
-				m_tmpSolverBodyPool[i].m_externalTorque*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep);
+				m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
 
 			if (infoGlobal.m_splitImpulse)
 				m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -42,7 +42,7 @@ protected:
 	btAlignedObjectArray<int>	m_orderFrictionConstraintPool;
 	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
 	int							m_maxOverrideNumSolverIterations;
-
+	int m_fixedBodyId;
 	void setupFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
 									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
 									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
@@ -56,10 +56,11 @@ protected:
 	btSolverConstraint&	addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
 	btSolverConstraint&	addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
 
-
+	
 	void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, 
-								const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation, 
-								btVector3& rel_pos1, btVector3& rel_pos2);
+								const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
+
+	static void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
 
 	void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, 
 										 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
@@ -70,6 +71,8 @@ protected:
 	
 	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
 
+	virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
+
 	void	convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
 
 
@@ -82,8 +85,8 @@ protected:
         const btSolverConstraint& contactConstraint);
 
 	//internal method
-	int		getOrInitSolverBody(btCollisionObject& body);
-	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
+	int		getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
+	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
 
 	void	resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
 
@@ -98,7 +101,7 @@ protected:
 	
 	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
 	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
-	btScalar solveSingleIteration(int iteration, 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);
 
 	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);
@@ -131,6 +134,11 @@ public:
 		return m_btSeed2;
 	}
 
+	
+	virtual btConstraintSolverType	getSolverType() const
+	{
+		return BT_SEQUENTIAL_IMPULSE_SOLVER;
+	}
 };
 
 

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h

@@ -25,7 +25,13 @@ TODO:
 #ifndef BT_SLIDER_CONSTRAINT_H
 #define BT_SLIDER_CONSTRAINT_H
 
-
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btSliderConstraintData2		btSliderConstraintDoubleData
+#define btSliderConstraintDataName  "btSliderConstraintDoubleData"
+#else
+#define btSliderConstraintData2		btSliderConstraintData 
+#define btSliderConstraintDataName	"btSliderConstraintData"
+#endif //BT_USE_DOUBLE_PRECISION
 
 #include "LinearMath/btVector3.h"
 #include "btJacobianEntry.h"
@@ -283,7 +289,10 @@ public:
 
 };
 
+
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+
+
 struct btSliderConstraintData
 {
 	btTypedConstraintData	m_typeConstraintData;
@@ -302,31 +311,48 @@ struct btSliderConstraintData
 };
 
 
+struct btSliderConstraintDoubleData
+{
+	btTypedConstraintDoubleData	m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTransformDoubleData m_rbBFrame;
+	
+	double	m_linearUpperLimit;
+	double	m_linearLowerLimit;
+
+	double	m_angularUpperLimit;
+	double	m_angularLowerLimit;
+
+	int	m_useLinearReferenceFrameA;
+	int m_useOffsetForConstraintFrame;
+
+};
+
 SIMD_FORCE_INLINE		int	btSliderConstraint::calculateSerializeBufferSize() const
 {
-	return sizeof(btSliderConstraintData);
+	return sizeof(btSliderConstraintData2);
 }
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
 SIMD_FORCE_INLINE	const char*	btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 
-	btSliderConstraintData* sliderData = (btSliderConstraintData*) dataBuffer;
+	btSliderConstraintData2* sliderData = (btSliderConstraintData2*) dataBuffer;
 	btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
 
-	m_frameInA.serializeFloat(sliderData->m_rbAFrame);
-	m_frameInB.serializeFloat(sliderData->m_rbBFrame);
+	m_frameInA.serialize(sliderData->m_rbAFrame);
+	m_frameInB.serialize(sliderData->m_rbBFrame);
 
-	sliderData->m_linearUpperLimit = float(m_upperLinLimit);
-	sliderData->m_linearLowerLimit = float(m_lowerLinLimit);
+	sliderData->m_linearUpperLimit = m_upperLinLimit;
+	sliderData->m_linearLowerLimit = m_lowerLinLimit;
 
-	sliderData->m_angularUpperLimit = float(m_upperAngLimit);
-	sliderData->m_angularLowerLimit = float(m_lowerAngLimit);
+	sliderData->m_angularUpperLimit = m_upperAngLimit;
+	sliderData->m_angularLowerLimit = m_lowerAngLimit;
 
 	sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA;
 	sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame;
 
-	return "btSliderConstraintData";
+	return btSliderConstraintDataName;
 }
 
 

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h

@@ -118,8 +118,8 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 	btVector3		m_turnVelocity;
 	btVector3		m_linearVelocity;
 	btVector3		m_angularVelocity;
-	btVector3		m_externalForce;
-	btVector3		m_externalTorque;
+	btVector3		m_externalForceImpulse;
+	btVector3		m_externalTorqueImpulse;
 
 	btRigidBody*	m_originalBody;
 	void	setWorldTransform(const btTransform& worldTransform)
@@ -132,6 +132,17 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 		return m_worldTransform;
 	}
 	
+	
+
+	SIMD_FORCE_INLINE void	getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity ) const
+	{
+		if (m_originalBody)
+			velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity+m_externalTorqueImpulse).cross(rel_pos);
+		else
+			velocity.setValue(0,0,0);
+	}
+
+
 	SIMD_FORCE_INLINE void	getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
 	{
 		if (m_originalBody)

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h

@@ -55,6 +55,7 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
 	{
 		void*		m_originalContactPoint;
 		btScalar	m_unusedPadding4;
+		int			m_numRowsForNonContactConstraint;
 	};
 
 	int	m_overrideNumSolverIterations;

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

@@ -109,7 +109,7 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
 const char*	btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btTypedConstraintData* tcd = (btTypedConstraintData*) dataBuffer;
+	btTypedConstraintData2* tcd = (btTypedConstraintData2*) dataBuffer;
 
 	tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
 	tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
@@ -123,14 +123,14 @@ const char*	btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
 	tcd->m_objectType = m_objectType;
 	tcd->m_needsFeedback = m_needsFeedback;
 	tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations;
-	tcd->m_breakingImpulseThreshold = float(m_breakingImpulseThreshold);
+	tcd->m_breakingImpulseThreshold = m_breakingImpulseThreshold;
 	tcd->m_isEnabled = m_isEnabled? 1: 0;
 	
 	tcd->m_userConstraintId =m_userConstraintId;
 	tcd->m_userConstraintType =m_userConstraintType;
 
-	tcd->m_appliedImpulse = float(m_appliedImpulse);
-	tcd->m_dbgDrawSize = float(m_dbgDrawSize );
+	tcd->m_appliedImpulse = m_appliedImpulse;
+	tcd->m_dbgDrawSize = m_dbgDrawSize;
 
 	tcd->m_disableCollisionsBetweenLinkedBodies = false;
 
@@ -142,7 +142,7 @@ const char*	btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
 		if (m_rbB.getConstraintRef(i) == this)
 			tcd->m_disableCollisionsBetweenLinkedBodies = true;
 
-	return "btTypedConstraintData";
+	return btTypedConstraintDataName;
 }
 
 btRigidBody& btTypedConstraint::getFixedBody()

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h

@@ -21,6 +21,15 @@ subject to the following restrictions:
 #include "btSolverConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btTypedConstraintData2		btTypedConstraintDoubleData
+#define btTypedConstraintDataName	"btTypedConstraintDoubleData"
+#else
+#define btTypedConstraintData2 		btTypedConstraintFloatData
+#define btTypedConstraintDataName  "btTypedConstraintFloatData" 
+#endif //BT_USE_DOUBLE_PRECISION
+
+
 class btSerializer;
 
 //Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
@@ -34,6 +43,7 @@ enum btTypedConstraintType
 	CONTACT_CONSTRAINT_TYPE,
 	D6_SPRING_CONSTRAINT_TYPE,
 	GEAR_CONSTRAINT_TYPE,
+	FIXED_CONSTRAINT_TYPE,
 	MAX_CONSTRAINT_TYPE
 };
 
@@ -356,6 +366,33 @@ SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScal
 }
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btTypedConstraintFloatData
+{
+	btRigidBodyFloatData		*m_rbA;
+	btRigidBodyFloatData		*m_rbB;
+	char	*m_name;
+
+	int	m_objectType;
+	int	m_userConstraintType;
+	int	m_userConstraintId;
+	int	m_needsFeedback;
+
+	float	m_appliedImpulse;
+	float	m_dbgDrawSize;
+
+	int	m_disableCollisionsBetweenLinkedBodies;
+	int	m_overrideNumSolverIterations;
+
+	float	m_breakingImpulseThreshold;
+	int		m_isEnabled;
+	
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+
+#define BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+///this structure is not used, except for loading pre-2.82 .bullet files
 struct	btTypedConstraintData
 {
 	btRigidBodyData		*m_rbA;
@@ -377,10 +414,35 @@ struct	btTypedConstraintData
 	int		m_isEnabled;
 	
 };
+#endif //BACKWARDS_COMPATIBLE
+
+struct	btTypedConstraintDoubleData
+{
+	btRigidBodyDoubleData		*m_rbA;
+	btRigidBodyDoubleData		*m_rbB;
+	char	*m_name;
+
+	int	m_objectType;
+	int	m_userConstraintType;
+	int	m_userConstraintId;
+	int	m_needsFeedback;
+
+	double	m_appliedImpulse;
+	double	m_dbgDrawSize;
+
+	int	m_disableCollisionsBetweenLinkedBodies;
+	int	m_overrideNumSolverIterations;
+
+	double	m_breakingImpulseThreshold;
+	int		m_isEnabled;
+	char	padding[4];
+	
+};
+
 
 SIMD_FORCE_INLINE	int	btTypedConstraint::calculateSerializeBufferSize() const
 {
-	return sizeof(btTypedConstraintData);
+	return sizeof(btTypedConstraintData2);
 }
 
 

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -209,7 +209,9 @@ m_gravity(0,-10,0),
 m_localTime(0),
 m_synchronizeAllMotionStates(false),
 m_applySpeculativeContactRestitution(false),
-m_profileTimings(0)
+m_profileTimings(0),
+m_fixedTimeStep(0),
+m_latencyMotionStateInterpolation(true)
 
 {
 	if (!m_constraintSolver)
@@ -358,7 +360,9 @@ void	btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
 		{
 			btTransform interpolatedTransform;
 			btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
-				body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
+				body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),
+				(m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(),
+				interpolatedTransform);
 			body->getMotionState()->setWorldTransform(interpolatedTransform);
 		}
 	}
@@ -402,6 +406,7 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 	if (maxSubSteps)
 	{
 		//fixed timestep with interpolation
+		m_fixedTimeStep = fixedTimeStep;
 		m_localTime += timeStep;
 		if (m_localTime >= fixedTimeStep)
 		{
@@ -412,7 +417,8 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 	{
 		//variable timestep
 		fixedTimeStep = timeStep;
-		m_localTime = timeStep;
+		m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep;
+		m_fixedTimeStep = 0;
 		if (btFuzzyZero(timeStep))
 		{
 			numSimulationSubSteps = 0;
@@ -742,12 +748,7 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
             if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
                 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
             {
-                if (colObj0->isActive() || colObj1->isActive())
-                {
-                    
-                    getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
-                                                                       (colObj1)->getIslandTag());
-                }
+				getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
             }
         }
     }
@@ -766,12 +767,7 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 				if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
 					((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
 				{
-					if (colObj0->isActive() || colObj1->isActive())
-					{
-
-						getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
-							(colObj1)->getIslandTag());
-					}
+					getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
 				}
 			}
 		}
@@ -1127,7 +1123,6 @@ void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 		{
 			//don't integrate/update velocities here, it happens in the constraint solver
 
-			//damping
 			body->applyDamping(timeStep);
 
 			body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -53,6 +53,7 @@ protected:
 
 	//for variable timesteps
 	btScalar	m_localTime;
+	btScalar	m_fixedTimeStep;
 	//for variable timesteps
 
 	bool	m_ownsIslandManager;
@@ -64,6 +65,8 @@ protected:
 	
 	int	m_profileTimings;
 
+	bool	m_latencyMotionStateInterpolation;
+
 	btAlignedObjectArray<btPersistentManifold*>	m_predictiveManifolds;
 
 	virtual void	predictUnconstraintMotion(btScalar timeStep);
@@ -74,7 +77,7 @@ protected:
 
 	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
 	
-	void	updateActivationState(btScalar timeStep);
+	virtual void	updateActivationState(btScalar timeStep);
 
 	void	updateActions(btScalar timeStep);
 
@@ -216,6 +219,16 @@ public:
 	///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo)
 	virtual	void	serialize(btSerializer* serializer);
 
+	///Interpolate motion state between previous and current transform, instead of current and next transform.
+	///This can relieve discontinuities in the rendering, due to penetrations
+	void setLatencyMotionStateInterpolation(bool latencyInterpolation )
+	{
+		m_latencyMotionStateInterpolation = latencyInterpolation;
+	}
+	bool getLatencyMotionStateInterpolation() const
+	{
+		return m_latencyMotionStateInterpolation;
+	}
 };
 
 #endif //BT_DISCRETE_DYNAMICS_WORLD_H

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -0,0 +1,1009 @@
+/*
+ * 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, <[email protected]>, 2011-2013
+ *   Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
+
+ 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"
+
+// #define INCLUDE_GYRO_TERM 
+
+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
+}
+
+
+
+
+//
+// Various spatial helper functions
+//
+
+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;
+    }
+
+    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);
+    }
+}
+
+
+//
+// Implementation of class btMultiBody
+//
+
+btMultiBody::btMultiBody(int n_links,
+                     btScalar mass,
+                     const btVector3 &inertia,
+                     bool fixed_base_,
+                     bool can_sleep_)
+    : base_quat(0, 0, 0, 1),
+      base_mass(mass),
+      base_inertia(inertia),
+    
+		fixed_base(fixed_base_),
+		awake(true),
+		can_sleep(can_sleep_),
+		sleep_timer(0),
+		m_baseCollider(0),
+		m_linearDamping(0.04f),
+		m_angularDamping(0.04f),
+		m_useGyroTerm(true),
+		m_maxAppliedImpulse(1000.f),
+		m_hasSelfCollision(true)
+{
+	 links.resize(n_links);
+
+	vector_buf.resize(2*n_links);
+    matrix_buf.resize(n_links + 1);
+	m_real_buf.resize(6 + 2*n_links);
+    base_pos.setValue(0, 0, 0);
+    base_force.setValue(0, 0, 0);
+    base_torque.setValue(0, 0, 0);
+}
+
+btMultiBody::~btMultiBody()
+{
+}
+
+void btMultiBody::setupPrismatic(int i,
+                               btScalar mass,
+                               const btVector3 &inertia,
+                               int parent,
+                               const btQuaternion &rot_parent_to_this,
+                               const btVector3 &joint_axis,
+                               const btVector3 &r_vector_when_q_zero,
+							   bool disableParentCollision)
+{
+    links[i].mass = mass;
+    links[i].inertia = inertia;
+    links[i].parent = parent;
+    links[i].zero_rot_parent_to_this = rot_parent_to_this;
+    links[i].axis_top.setValue(0,0,0);
+    links[i].axis_bottom = joint_axis;
+    links[i].e_vector = r_vector_when_q_zero;
+    links[i].is_revolute = false;
+    links[i].cached_rot_parent_to_this = rot_parent_to_this;
+	if (disableParentCollision)
+		links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+
+    links[i].updateCache();
+}
+
+void btMultiBody::setupRevolute(int i,
+                              btScalar mass,
+                              const btVector3 &inertia,
+                              int parent,
+                              const btQuaternion &zero_rot_parent_to_this,
+                              const btVector3 &joint_axis,
+                              const btVector3 &parent_axis_position,
+                              const btVector3 &my_axis_position,
+							  bool disableParentCollision)
+{
+    links[i].mass = mass;
+    links[i].inertia = inertia;
+    links[i].parent = parent;
+    links[i].zero_rot_parent_to_this = zero_rot_parent_to_this;
+    links[i].axis_top = joint_axis;
+    links[i].axis_bottom = joint_axis.cross(my_axis_position);
+    links[i].d_vector = my_axis_position;
+    links[i].e_vector = parent_axis_position;
+    links[i].is_revolute = true;
+	if (disableParentCollision)
+		links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+    links[i].updateCache();
+}
+
+
+
+
+	
+int btMultiBody::getParent(int i) const
+{
+    return links[i].parent;
+}
+
+btScalar btMultiBody::getLinkMass(int i) const
+{
+    return links[i].mass;
+}
+
+const btVector3 & btMultiBody::getLinkInertia(int i) const
+{
+    return links[i].inertia;
+}
+
+btScalar btMultiBody::getJointPos(int i) const
+{
+    return links[i].joint_pos;
+}
+
+btScalar btMultiBody::getJointVel(int i) const
+{
+    return m_real_buf[6 + i];
+}
+
+void btMultiBody::setJointPos(int i, btScalar q)
+{
+    links[i].joint_pos = q;
+    links[i].updateCache();
+}
+
+void btMultiBody::setJointVel(int i, btScalar qdot)
+{
+    m_real_buf[6 + i] = qdot;
+}
+
+const btVector3 & btMultiBody::getRVector(int i) const
+{
+    return links[i].cached_r_vector;
+}
+
+const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
+{
+    return links[i].cached_rot_parent_to_this;
+}
+
+btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
+{
+    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
+{
+    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
+{
+    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
+{
+    if (i == -1) {
+        return quatRotate(getWorldToBaseRot(), world_dir);
+    } else {
+        return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir));
+    }
+}
+
+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(base_quat ,getBaseOmega());
+    vel[0] = quatRotate(base_quat ,getBaseVel());
+    
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = links[i].parent;
+
+        // transform parent vel into this frame, store in omega[i+1], vel[i+1]
+        SpatialTransform(btMatrix3x3(links[i].cached_rot_parent_to_this), links[i].cached_r_vector,
+                         omega[parent+1], vel[parent+1],
+                         omega[i+1], vel[i+1]);
+
+        // now add qidot * shat_i
+        omega[i+1] += getJointVel(i) * links[i].axis_top;
+        vel[i+1] += getJointVel(i) * links[i].axis_bottom;
+    }
+}
+
+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 = base_mass * vel[0].dot(vel[0]);
+    result += omega[0].dot(base_inertia * omega[0]);
+    
+    for (int i = 0; i < num_links; ++i) {
+        result += links[i].mass * vel[i+1].dot(vel[i+1]);
+        result += omega[i+1].dot(links[i].inertia * 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] = base_quat;
+    btVector3 result = quatRotate(rot_from_world[0].inverse() , (base_inertia * omega[0]));
+
+    for (int i = 0; i < num_links; ++i) {
+        rot_from_world[i+1] = links[i].cached_rot_parent_to_this * rot_from_world[links[i].parent+1];
+        result += (quatRotate(rot_from_world[i+1].inverse() , (links[i].inertia * omega[i+1])));
+    }
+
+    return result;
+}
+
+
+void btMultiBody::clearForcesAndTorques()
+{
+    base_force.setValue(0, 0, 0);
+    base_torque.setValue(0, 0, 0);
+
+    for (int i = 0; i < getNumLinks(); ++i) {
+        links[i].applied_force.setValue(0, 0, 0);
+        links[i].applied_torque.setValue(0, 0, 0);
+        links[i].joint_torque = 0;
+    }
+}
+
+void btMultiBody::clearVelocities()
+{
+	for (int i = 0; i < 6 + getNumLinks(); ++i) 
+	{
+		m_real_buf[i] = 0.f;
+	}
+}
+void btMultiBody::addLinkForce(int i, const btVector3 &f)
+{
+    links[i].applied_force += f;
+}
+
+void btMultiBody::addLinkTorque(int i, const btVector3 &t)
+{
+    links[i].applied_torque += t;
+}
+
+void btMultiBody::addJointTorque(int i, btScalar Q)
+{
+    links[i].joint_torque += Q;
+}
+
+const btVector3 & btMultiBody::getLinkForce(int i) const
+{
+    return links[i].applied_force;
+}
+
+const btVector3 & btMultiBody::getLinkTorque(int i) const
+{
+    return links[i].applied_torque;
+}
+
+btScalar btMultiBody::getJointTorque(int i) const
+{
+    return links[i].joint_torque;
+}
+
+
+inline btMatrix3x3 vecMulVecTranspose(const btVector3& v0, const btVector3& v1Transposed)
+{
+		btVector3 row0 = btVector3( 
+			v0.x() * v1Transposed.x(),
+			v0.x() * v1Transposed.y(),
+			v0.x() * v1Transposed.z());
+		btVector3 row1 = btVector3( 
+			v0.y() * v1Transposed.x(),
+			v0.y() * v1Transposed.y(),
+			v0.y() * v1Transposed.z());
+		btVector3 row2 = btVector3( 
+			v0.z() * v1Transposed.x(),
+			v0.z() * v1Transposed.y(),
+			v0.z() * v1Transposed.z());
+
+        btMatrix3x3 m(row0[0],row0[1],row0[2],
+						row1[0],row1[1],row1[2],
+						row2[0],row2[1],row2[2]);
+		return m;
+}
+
+
+void btMultiBody::stepVelocities(btScalar dt,
+                               btAlignedObjectArray<btScalar> &scratch_r,
+                               btAlignedObjectArray<btVector3> &scratch_v,
+                               btAlignedObjectArray<btMatrix3x3> &scratch_m)
+{
+    // 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.
+    
+	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*num_links + 6);
+    scratch_v.resize(8*num_links + 6);
+    scratch_m.resize(4*num_links + 4);
+
+    btScalar * r_ptr = &scratch_r[0];
+    btScalar * output = &scratch_r[num_links];  // "output" holds the q_double_dot results
+    btVector3 * v_ptr = &scratch_v[0];
+    
+    // vhat_i  (top = angular, bottom = linear part)
+    btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
+    btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
+
+    // zhat_i^A
+    btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
+    btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
+
+    // chat_i  (note NOT defined for the base)
+    btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
+    btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
+
+    // top left, top right and bottom left blocks of Ihat_i^A.
+    // bottom right block = transpose of top left block and is not stored.
+    // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
+    btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
+    btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
+    btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
+
+    // Cached 3x3 rotation matrices from parent frame to this frame.
+    btMatrix3x3 * rot_from_parent = &matrix_buf[0];
+    btMatrix3x3 * rot_from_world = &scratch_m[0];
+
+    // hhat_i, ahat_i
+    // hhat is NOT stored for the base (but ahat is)
+    btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0;
+    btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0;
+    btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
+    btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
+
+    // Y_i, D_i
+    btScalar * Y = r_ptr; r_ptr += num_links;
+    btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0;
+
+    // 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(base_quat);
+
+    vel_top_angular[0] = rot_from_parent[0] * base_omega;
+    vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
+
+    if (fixed_base) {
+        zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
+    } else {
+        zero_acc_top_angular[0] = - (rot_from_parent[0] * (base_force 
+                                                   - base_mass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
+		
+        zero_acc_bottom_linear[0] =
+            - (rot_from_parent[0] * base_torque);
+
+		if (m_useGyroTerm)
+			zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( base_inertia * vel_top_angular[0] );
+
+        zero_acc_bottom_linear[0] += base_inertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
+
+    }
+
+
+
+    inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
+	
+	
+    inertia_top_right[0].setValue(base_mass, 0, 0,
+                            0, base_mass, 0,
+                            0, 0, base_mass);
+    inertia_bottom_left[0].setValue(base_inertia[0], 0, 0,
+                              0, base_inertia[1], 0,
+                              0, 0, base_inertia[2]);
+
+    rot_from_world[0] = rot_from_parent[0];
+
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = links[i].parent;
+        rot_from_parent[i+1] = btMatrix3x3(links[i].cached_rot_parent_to_this);
+		
+
+        rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
+        
+        // vhat_i = i_xhat_p(i) * vhat_p(i)
+        SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
+                         vel_top_angular[parent+1], vel_bottom_linear[parent+1],
+                         vel_top_angular[i+1], vel_bottom_linear[i+1]);
+
+        // we can now calculate chat_i
+        // remember vhat_i is really vhat_p(i) (but in current frame) at this point
+        coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(links[i].cached_r_vector))
+            + 2 * vel_top_angular[i+1].cross(links[i].axis_bottom) * getJointVel(i);
+        if (links[i].is_revolute) {
+            coriolis_top_angular[i] = vel_top_angular[i+1].cross(links[i].axis_top) * getJointVel(i);
+            coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * links[i].axis_top.cross(links[i].axis_bottom);
+        } else {
+            coriolis_top_angular[i] = btVector3(0,0,0);
+        }
+        
+        // now set vhat_i to its true value by doing
+        // vhat_i += qidot * shat_i
+        vel_top_angular[i+1] += getJointVel(i) * links[i].axis_top;
+        vel_bottom_linear[i+1] += getJointVel(i) * links[i].axis_bottom;
+
+        // calculate zhat_i^A
+        zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (links[i].applied_force));
+        zero_acc_top_angular[i+1] += links[i].mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
+
+        zero_acc_bottom_linear[i+1] =
+            - (rot_from_world[i+1] * links[i].applied_torque);
+		if (m_useGyroTerm)
+		{
+			zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( links[i].inertia * vel_top_angular[i+1] );
+		}
+
+        zero_acc_bottom_linear[i+1] += links[i].inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
+
+        // calculate Ihat_i^A
+        inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
+        inertia_top_right[i+1].setValue(links[i].mass, 0, 0,
+                                  0, links[i].mass, 0,
+                                  0, 0, links[i].mass);
+        inertia_bottom_left[i+1].setValue(links[i].inertia[0], 0, 0,
+                                    0, links[i].inertia[1], 0,
+                                    0, 0, links[i].inertia[2]);
+    }
+
+
+    // 'Downward' loop.
+    // (part of TreeForwardDynamics in Mirtich.)
+    for (int i = num_links - 1; i >= 0; --i) {
+
+        h_top[i] = inertia_top_left[i+1] * links[i].axis_top + inertia_top_right[i+1] * links[i].axis_bottom;
+        h_bottom[i] = inertia_bottom_left[i+1] * links[i].axis_top + inertia_top_left[i+1].transpose() * links[i].axis_bottom;
+		btScalar val = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]);
+        D[i] = val;
+		Y[i] = links[i].joint_torque
+            - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1])
+            - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
+
+        const int parent = links[i].parent;
+
+        
+        // Ip += pXi * (Ii - hi hi' / Di) * iXp
+        const btScalar one_over_di = 1.0f / D[i];
+
+		
+
+
+		const btMatrix3x3 TL = inertia_top_left[i+1]   - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]);
+        const btMatrix3x3 TR = inertia_top_right[i+1]  - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]);
+        const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]);
+
+
+        btMatrix3x3 r_cross;
+        r_cross.setValue(
+            0, -links[i].cached_r_vector[2], links[i].cached_r_vector[1],
+            links[i].cached_r_vector[2], 0, -links[i].cached_r_vector[0],
+            -links[i].cached_r_vector[1], links[i].cached_r_vector[0], 0);
+        
+        inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
+        inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
+        inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
+            (r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];
+        
+        
+        // Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
+        btVector3 in_top, in_bottom, out_top, out_bottom;
+        const btScalar Y_over_D = Y[i] * one_over_di;
+        in_top = zero_acc_top_angular[i+1]
+            + inertia_top_left[i+1] * coriolis_top_angular[i]
+            + inertia_top_right[i+1] * coriolis_bottom_linear[i]
+            + Y_over_D * h_top[i];
+        in_bottom = zero_acc_bottom_linear[i+1]
+            + inertia_bottom_left[i+1] * coriolis_top_angular[i]
+            + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
+            + Y_over_D * h_bottom[i];
+        InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
+                                in_top, in_bottom, out_top, out_bottom);
+        zero_acc_top_angular[parent+1] += out_top;
+        zero_acc_bottom_linear[parent+1] += out_bottom;
+    }
+
+
+    // Second 'upward' loop
+    // (part of TreeForwardDynamics in Mirtich)
+
+    if (fixed_base) 
+	{
+        accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
+    } 
+	else 
+	{
+        if (num_links > 0) 
+		{
+            //Matrix<btScalar, 6, 6> Imatrix;
+            //Imatrix.block<3,3>(0,0) = inertia_top_left[0];
+            //Imatrix.block<3,3>(3,0) = inertia_bottom_left[0];
+            //Imatrix.block<3,3>(0,3) = inertia_top_right[0];
+            //Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose();
+            //cached_imatrix_lu.reset(new Eigen::LU<Matrix<btScalar, 6, 6> >(Imatrix));  // TODO: Avoid memory allocation here?
+
+			cached_inertia_top_left = inertia_top_left[0];
+			cached_inertia_top_right = inertia_top_right[0];
+			cached_inertia_lower_left = inertia_bottom_left[0];
+			cached_inertia_lower_right= inertia_top_left[0].transpose();
+
+        }
+		btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
+		btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
+        float result[6];
+
+		solveImatrix(rhs_top, rhs_bot, result);
+//		printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
+        for (int i = 0; i < 3; ++i) {
+            accel_top[0][i] = -result[i];
+            accel_bottom[0][i] = -result[i+3];
+        }
+
+    }
+
+    // now do the loop over the links
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = links[i].parent;
+        SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
+                         accel_top[parent+1], accel_bottom[parent+1],
+                         accel_top[i+1], accel_bottom[i+1]);
+        joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
+        accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * links[i].axis_top;
+        accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * links[i].axis_bottom;
+    }
+
+    // transform base accelerations back to the world frame.
+    btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
+	output[0] = omegadot_out[0];
+	output[1] = omegadot_out[1];
+	output[2] = omegadot_out[2];
+
+    btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
+	output[3] = vdot_out[0];
+	output[4] = vdot_out[1];
+	output[5] = vdot_out[2];
+    // Final step: add the accelerations (times dt) to the velocities.
+    applyDeltaVee(output, dt);
+
+	
+}
+
+
+
+void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float 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 links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
+        result[0] = rhs_bot[0] / base_inertia[0];
+        result[1] = rhs_bot[1] / base_inertia[1];
+        result[2] = rhs_bot[2] / base_inertia[2];
+        result[3] = rhs_top[0] / base_mass;
+        result[4] = rhs_top[1] / base_mass;
+        result[5] = rhs_top[2] / base_mass;
+    } else 
+	{
+		/// Special routine for calculating the inverse of a spatial inertia matrix
+		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
+		btMatrix3x3 Binv = cached_inertia_top_right.inverse()*-1.f;
+		btMatrix3x3 tmp = cached_inertia_lower_right * Binv;
+		btMatrix3x3 invIupper_right = (tmp * cached_inertia_top_left + cached_inertia_lower_left).inverse();
+		tmp = invIupper_right * cached_inertia_lower_right;
+		btMatrix3x3 invI_upper_left = (tmp * Binv);
+		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
+		tmp = cached_inertia_top_left  * 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::calcAccelerationDeltas(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(num_links);
+    scratch_v.resize(4*num_links + 4);
+
+    btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0];
+    btVector3 * v_ptr = &scratch_v[0];
+    
+    // zhat_i^A (scratch space)
+    btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
+    btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
+
+    // rot_from_parent (cached from calcAccelerations)
+    const btMatrix3x3 * rot_from_parent = &matrix_buf[0];
+
+    // hhat (cached), accel (scratch)
+    const btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0;
+    const btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0;
+    btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
+    btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
+
+    // Y_i (scratch), D_i (cached)
+    btScalar * Y = r_ptr; r_ptr += num_links;
+    const btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0;
+
+    btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size());
+    btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
+
+
+    
+    // First 'upward' loop.
+    // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
+
+    btVector3 input_force(force[3],force[4],force[5]);
+    btVector3 input_torque(force[0],force[1],force[2]);
+    
+    // Fill in zero_acc
+    // -- set to force/torque on the base, zero otherwise
+    if (fixed_base) 
+	{
+        zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
+    } else 
+	{
+        zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force);
+        zero_acc_bottom_linear[0] =  - (rot_from_parent[0] * input_torque);
+    }
+    for (int i = 0; i < num_links; ++i) 
+	{
+        zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0);
+    }
+
+    // 'Downward' loop.
+    for (int i = num_links - 1; i >= 0; --i) 
+	{
+
+        Y[i] = - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
+        Y[i] += force[6 + i];  // add joint torque
+        
+        const int parent = links[i].parent;
+        
+        // Zp += pXi * (Zi + hi*Yi/Di)
+        btVector3 in_top, in_bottom, out_top, out_bottom;
+        const btScalar Y_over_D = Y[i] / D[i];
+        in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i];
+        in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i];
+        InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
+                                in_top, in_bottom, out_top, out_bottom);
+        zero_acc_top_angular[parent+1] += out_top;
+        zero_acc_bottom_linear[parent+1] += out_bottom;
+    }
+
+    // ptr to the joint accel part of the output
+    btScalar * joint_accel = output + 6;
+
+    // Second 'upward' loop
+    if (fixed_base) 
+	{
+        accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
+    } else 
+	{
+		btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
+		btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
+		
+		float result[6];
+        solveImatrix(rhs_top,rhs_bot, result);
+	//	printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
+
+        for (int i = 0; i < 3; ++i) {
+            accel_top[0][i] = -result[i];
+            accel_bottom[0][i] = -result[i+3];
+        }
+
+    }
+    
+    // now do the loop over the links
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = links[i].parent;
+        SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
+                         accel_top[parent+1], accel_bottom[parent+1],
+                         accel_top[i+1], accel_bottom[i+1]);
+        joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
+        accel_top[i+1] += joint_accel[i] * links[i].axis_top;
+        accel_bottom[i+1] += joint_accel[i] * links[i].axis_bottom;
+    }
+
+    // transform base accelerations back to the world frame.
+    btVector3 omegadot_out;
+    omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
+	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() * accel_bottom[0];
+
+	output[3] = vdot_out[0];
+	output[4] = vdot_out[1];
+	output[5] = vdot_out[2];
+}
+
+void btMultiBody::stepPositions(btScalar dt)
+{
+	int num_links = getNumLinks();
+    // step position by adding dt * velocity
+	btVector3 v = getBaseVel();
+    base_pos += dt * v;
+
+    // "exponential map" method for the rotation
+    btVector3 base_omega = getBaseOmega();
+    const btScalar omega_norm = base_omega.norm();
+    const btScalar omega_times_dt = omega_norm * dt;
+    const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156
+    if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE) 
+	{
+        const btScalar xsq = omega_times_dt * omega_times_dt;     // |omega|^2 * dt^2
+        const btScalar sin_term = dt * (xsq / 48.0f - 0.5f);      // -sin(0.5*dt*|omega|) / |omega|
+        const btScalar cos_term = 1.0f - xsq / 8.0f;              // cos(0.5*dt*|omega|)
+        base_quat = base_quat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term);
+    } else 
+	{
+        base_quat = base_quat * btQuaternion(base_omega / omega_norm,-omega_times_dt);
+    }
+
+    // Make sure the quaternion represents a valid rotation.
+    // (Not strictly necessary, but helps prevent any round-off errors from building up.)
+    base_quat.normalize();
+
+    // Finally we can update joint_pos for each of the links
+    for (int i = 0; i < num_links; ++i) 
+	{
+		float jointVel = getJointVel(i);
+        links[i].joint_pos += dt * jointVel;
+        links[i].updateCache();
+    }
+}
+
+void btMultiBody::fillContactJacobian(int link,
+                                    const btVector3 &contact_point,
+                                    const btVector3 &normal,
+                                    btScalar *jac,
+                                    btAlignedObjectArray<btScalar> &scratch_r,
+                                    btAlignedObjectArray<btVector3> &scratch_v,
+                                    btAlignedObjectArray<btMatrix3x3> &scratch_m) const
+{
+    // temporary space
+	int num_links = getNumLinks();
+    scratch_v.resize(2*num_links + 2);
+    scratch_m.resize(num_links + 1);
+
+    btVector3 * v_ptr = &scratch_v[0];
+    btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
+    btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
+    btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
+
+    scratch_r.resize(num_links);
+    btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
+
+    btMatrix3x3 * rot_from_world = &scratch_m[0];
+
+    const btVector3 p_minus_com_world = contact_point - base_pos;
+
+    rot_from_world[0] = btMatrix3x3(base_quat);
+
+    p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
+    n_local[0] = rot_from_world[0] * normal;
+    
+    // omega coeffients first.
+    btVector3 omega_coeffs;
+    omega_coeffs = p_minus_com_world.cross(normal);
+	jac[0] = omega_coeffs[0];
+	jac[1] = omega_coeffs[1];
+	jac[2] = omega_coeffs[2];
+    // then v coefficients
+    jac[3] = normal[0];
+    jac[4] = normal[1];
+    jac[5] = normal[2];
+
+    // Set remaining jac values to zero for now.
+    for (int i = 6; i < 6 + num_links; ++i) {
+        jac[i] = 0;
+    }
+
+    // Qdot coefficients, if necessary.
+    if (num_links > 0 && link > -1) {
+
+        // TODO: speed this up -- don't calculate for 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 = links[i].parent;
+            const btMatrix3x3 mtx(links[i].cached_rot_parent_to_this);
+            rot_from_world[i+1] = mtx * rot_from_world[parent+1];
+
+            n_local[i+1] = mtx * n_local[parent+1];
+            p_minus_com[i+1] = mtx * p_minus_com[parent+1] - links[i].cached_r_vector;
+
+            // calculate the jacobian entry
+            if (links[i].is_revolute) {
+                results[i] = n_local[i+1].dot( links[i].axis_top.cross(p_minus_com[i+1]) + links[i].axis_bottom );
+            } else {
+                results[i] = n_local[i+1].dot( links[i].axis_bottom );
+            }
+        }
+
+        // Now copy through to output.
+        while (link != -1) {
+            jac[6 + link] = results[link];
+            link = links[link].parent;
+        }
+    }
+}
+
+void btMultiBody::wakeUp()
+{
+    awake = true;
+}
+
+void btMultiBody::goToSleep()
+{
+    awake = false;
+}
+
+void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
+{
+	int num_links = getNumLinks();
+	extern bool gDisableDeactivation;
+    if (!can_sleep || gDisableDeactivation) 
+	{
+		awake = true;
+		sleep_timer = 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 + num_links; ++i) {
+        motion += m_real_buf[i] * m_real_buf[i];
+    }
+
+    if (motion < SLEEP_EPSILON) {
+        sleep_timer += timestep;
+        if (sleep_timer > SLEEP_TIMEOUT) {
+            goToSleep();
+        }
+    } else {
+        sleep_timer = 0;
+		if (!awake)
+			wakeUp();
+    }
+}

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBody.h

@@ -0,0 +1,466 @@
+/*
+ * 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, <[email protected]>, 2011-2013
+ *   Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
+
+ 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"
+
+
+#include "btMultiBodyLink.h"
+class btMultiBodyLinkCollider;
+
+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 fixed_base_,           // whether the base is fixed (true) or can move (false)
+              bool can_sleep_);
+
+    ~btMultiBody();
+    
+    void setupPrismatic(int i,             // 0 to num_links-1
+                        btScalar mass,
+                        const btVector3 &inertia,       // in my frame; assumed diagonal
+                        int parent,
+                        const btQuaternion &rot_parent_to_this,  // rotate points in parent frame to my frame.
+                        const btVector3 &joint_axis,             // in my frame
+                        const btVector3 &r_vector_when_q_zero,  // vector from parent COM to my COM, in my frame, when q = 0.
+						bool disableParentCollision=false
+						);
+
+    void setupRevolute(int i,            // 0 to num_links-1
+                       btScalar mass,
+                       const btVector3 &inertia,
+                       int parent,
+                       const btQuaternion &zero_rot_parent_to_this,  // rotate points in parent frame to this frame, when q = 0
+                       const btVector3 &joint_axis,    // in my frame
+                       const btVector3 &parent_axis_position,    // vector from parent COM to joint axis, in PARENT frame
+                       const btVector3 &my_axis_position,       // vector from joint axis to my COM, in MY frame
+					   bool disableParentCollision=false);
+	
+	const btMultibodyLink& getLink(int index) const
+	{
+		return links[index];
+	}
+
+	btMultibodyLink& getLink(int index)
+	{
+		return 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;
+	}
+
+    //
+    // 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 links, masses, moments of inertia
+    //
+
+    int getNumLinks() const { return links.size(); }
+    btScalar getBaseMass() const { return base_mass; }
+    const btVector3 & getBaseInertia() const { return base_inertia; }
+    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) { base_mass = mass; }
+    void setBaseInertia(const btVector3 &inertia) { base_inertia = inertia; }
+
+
+    //
+    // get/set pos/vel/rot/omega for the base link
+    //
+
+    const btVector3 & getBasePos() const { return base_pos; }    // in world frame
+    const btVector3 getBaseVel() const 
+	{ 
+		return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]); 
+	}     // in world frame
+    const btQuaternion & getWorldToBaseRot() const 
+	{ 
+		return base_quat; 
+	}     // rotates world vectors into base frame
+    btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); }   // in world frame
+
+    void setBasePos(const btVector3 &pos) 
+	{ 
+		base_pos = pos; 
+	}
+    void setBaseVel(const btVector3 &vel) 
+	{ 
+
+		m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2]; 
+	}
+    void setWorldToBaseRot(const btQuaternion &rot) 
+	{ 
+		base_quat = rot; 
+	}
+    void setBaseOmega(const btVector3 &omega) 
+	{ 
+		m_real_buf[0]=omega[0]; 
+		m_real_buf[1]=omega[1]; 
+		m_real_buf[2]=omega[2]; 
+	}
+
+
+    //
+    // get/set pos/vel for child links (i = 0 to num_links-1)
+    //
+
+    btScalar getJointPos(int i) const;
+    btScalar getJointVel(int i) const;
+
+    void setJointPos(int i, btScalar q);
+    void setJointVel(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_real_buf[0]; 
+	}
+/*    btScalar * getVelocityVector() 
+	{ 
+		return &real_buf[0]; 
+	}
+  */  
+
+    //
+    // get the frames of reference (positions and orientations) of the child 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;
+    
+
+    //
+    // 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 clearVelocities();
+
+    void addBaseForce(const btVector3 &f) 
+	{ 
+		base_force += f; 
+	}
+    void addBaseTorque(const btVector3 &t) { base_torque += t; }
+    void addLinkForce(int i, const btVector3 &f);
+    void addLinkTorque(int i, const btVector3 &t);
+    void addJointTorque(int i, btScalar Q);
+
+    const btVector3 & getBaseForce() const { return base_force; }
+    const btVector3 & getBaseTorque() const { return base_torque; }
+    const btVector3 & getLinkForce(int i) const;
+    const btVector3 & getLinkTorque(int i) const;
+    btScalar getJointTorque(int i) const;
+
+
+    //
+    // 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 stepVelocities(btScalar dt,
+                        btAlignedObjectArray<btScalar> &scratch_r,
+                        btAlignedObjectArray<btVector3> &scratch_v,
+                        btAlignedObjectArray<btMatrix3x3> &scratch_m);
+
+    // calcAccelerationDeltas
+    // input: force vector (in same format as jacobian, i.e.:
+    //                      3 torque values, 3 force values, num_links joint torque values)
+    // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
+    // (existing contents of output array are replaced)
+    // stepVelocities must have been called first.
+    void calcAccelerationDeltas(const btScalar *force, btScalar *output,
+                                btAlignedObjectArray<btScalar> &scratch_r,
+                                btAlignedObjectArray<btVector3> &scratch_v) const;
+
+    // apply a delta-vee directly. used in sequential impulses code.
+    void applyDeltaVee(const btScalar * delta_vee) 
+	{
+
+        for (int i = 0; i < 6 + getNumLinks(); ++i) 
+		{
+			m_real_buf[i] += delta_vee[i];
+		}
+		
+    }
+    void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) 
+	{
+		btScalar sum = 0;
+        for (int i = 0; i < 6 + getNumLinks(); ++i)
+		{
+			sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
+		}
+		btScalar l = btSqrt(sum);
+		/*
+		static btScalar maxl = -1e30f;
+		if (l>maxl)
+		{
+			maxl=l;
+	//		printf("maxl=%f\n",maxl);
+		}
+		*/
+		if (l>m_maxAppliedImpulse)
+		{
+//			printf("exceeds 100: l=%f\n",maxl);
+			multiplier *= m_maxAppliedImpulse/l;
+		}
+
+        for (int i = 0; i < 6 + getNumLinks(); ++i)
+		{
+			sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
+			m_real_buf[i] += delta_vee[i] * multiplier;
+		}
+    }
+
+    // timestep the positions (given current velocities).
+    void stepPositions(btScalar dt);
+
+
+    //
+    // contacts
+    //
+
+    // This routine fills out a contact constraint jacobian for this body.
+    // the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
+    // 'normal' & 'contact_point' are both given in world coordinates.
+    void fillContactJacobian(int link,
+                             const btVector3 &contact_point,
+                             const btVector3 &normal,
+                             btScalar *jac,
+                             btAlignedObjectArray<btScalar> &scratch_r,
+                             btAlignedObjectArray<btVector3> &scratch_v,
+                             btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
+
+
+    //
+    // sleeping
+    //
+	void	setCanSleep(bool canSleep)
+	{
+		can_sleep = canSleep;
+	}
+
+    bool isAwake() const { return awake; }
+    void wakeUp();
+    void goToSleep();
+    void checkMotionAndSleepIfRequired(btScalar timestep);
+    
+	bool hasFixedBase() const
+	{
+		    return fixed_base;
+	}
+
+	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 links, make sure to re-initialize or update existing links
+	{
+		links.resize(numLinks);
+	}
+
+	btScalar getLinearDamping() const
+	{
+			return m_linearDamping;
+	}
+	void setLinearDamping( btScalar damp)
+	{
+		m_linearDamping = damp;
+	}
+	btScalar getAngularDamping() const
+	{
+		return m_angularDamping;
+	}
+		
+	bool getUseGyroTerm() const
+	{
+		return m_useGyroTerm;
+	}
+	void setUseGyroTerm(bool useGyro)
+	{
+		m_useGyroTerm = useGyro;
+	}
+	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;
+	}
+
+private:
+    btMultiBody(const btMultiBody &);  // not implemented
+    void operator=(const btMultiBody &);  // not implemented
+
+    void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
+
+	void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
+    
+	
+private:
+
+	btMultiBodyLinkCollider* m_baseCollider;//can be NULL
+
+    btVector3 base_pos;       // position of COM of base (world frame)
+    btQuaternion base_quat;   // rotates world points into base frame
+
+    btScalar base_mass;         // mass of the base
+    btVector3 base_inertia;   // inertia of the base (in local frame; diagonal)
+
+    btVector3 base_force;     // external force applied to base. World frame.
+    btVector3 base_torque;    // external torque applied to base. World frame.
+    
+    btAlignedObjectArray<btMultibodyLink> links;    // array of links, excluding the base. index from 0 to num_links-1.
+	btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
+    
+    //
+    // real_buf:
+    //  offset         size            array
+    //   0              6 + num_links   v (base_omega; base_vel; joint_vels)
+    //   6+num_links    num_links       D
+    //
+    // vector_buf:
+    //  offset         size         array
+    //   0              num_links    h_top
+    //   num_links      num_links    h_bottom
+    //
+    // matrix_buf:
+    //  offset         size         array
+    //   0              num_links+1  rot_from_parent
+    //
+    
+    btAlignedObjectArray<btScalar> m_real_buf;
+    btAlignedObjectArray<btVector3> vector_buf;
+    btAlignedObjectArray<btMatrix3x3> matrix_buf;
+
+    //std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
+
+	btMatrix3x3 cached_inertia_top_left;
+	btMatrix3x3 cached_inertia_top_right;
+	btMatrix3x3 cached_inertia_lower_left;
+	btMatrix3x3 cached_inertia_lower_right;
+
+    bool fixed_base;
+
+    // Sleep parameters.
+    bool awake;
+    bool can_sleep;
+    btScalar sleep_timer;
+
+	int	m_companionId;
+	btScalar	m_linearDamping;
+	btScalar	m_angularDamping;
+	bool	m_useGyroTerm;
+	btScalar	m_maxAppliedImpulse;
+	bool		m_hasSelfCollision;
+};
+
+#endif

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -0,0 +1,527 @@
+#include "btMultiBodyConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
+btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
+	:m_bodyA(bodyA),
+	m_bodyB(bodyB),
+	m_linkA(linkA),
+	m_linkB(linkB),
+	m_num_rows(numRows),
+	m_isUnilateral(isUnilateral),
+	m_maxAppliedImpulse(100)
+{
+	m_jac_size_A = (6 + bodyA->getNumLinks());
+	m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0));
+	m_pos_offset = ((1 + m_jac_size_both)*m_num_rows);
+	m_data.resize((2 + m_jac_size_both) * m_num_rows);
+}
+
+btMultiBodyConstraint::~btMultiBodyConstraint()
+{
+}
+
+
+
+btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
+														btMultiBodyJacobianData& data,
+														btScalar* jacOrgA,btScalar* jacOrgB,
+														const btContactSolverInfo& infoGlobal,
+														btScalar desiredVelocity,
+														btScalar lowerLimit,
+														btScalar upperLimit)
+{
+			
+	
+	
+	constraintRow.m_multiBodyA = m_bodyA;
+	constraintRow.m_multiBodyB = m_bodyB;
+
+	btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
+	btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
+
+	if (multiBodyA)
+	{
+		
+		const int ndofA  = multiBodyA->getNumLinks() + 6;
+
+		constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
+
+		if (constraintRow.m_deltaVelAindex <0)
+		{
+			constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
+			multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
+			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
+		} else
+		{
+			btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
+		}
+
+		constraintRow.m_jacAindex = data.m_jacobians.size();
+		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
+		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+		for (int i=0;i<ndofA;i++)
+			data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
+		
+		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
+		multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+	} 
+
+	if (multiBodyB)
+	{
+		const int ndofB  = multiBodyB->getNumLinks() + 6;
+
+		constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
+		if (constraintRow.m_deltaVelBindex <0)
+		{
+			constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
+			multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
+			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
+		}
+
+		constraintRow.m_jacBindex = data.m_jacobians.size();
+		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
+
+		for (int i=0;i<ndofB;i++)
+			data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
+
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
+		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+		multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
+	} 
+	{
+						
+		btVector3 vec;
+		btScalar denom0 = 0.f;
+		btScalar denom1 = 0.f;
+		btScalar* jacB = 0;
+		btScalar* jacA = 0;
+		btScalar* lambdaA =0;
+		btScalar* lambdaB =0;
+		int ndofA  = 0;
+		if (multiBodyA)
+		{
+			ndofA  = multiBodyA->getNumLinks() + 6;
+			jacA = &data.m_jacobians[constraintRow.m_jacAindex];
+			lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
+			for (int i = 0; i < ndofA; ++i)
+			{
+				btScalar j = jacA[i] ;
+				btScalar l =lambdaA[i];
+				denom0 += j*l;
+			}
+		} 
+		if (multiBodyB)
+		{
+			const int ndofB  = multiBodyB->getNumLinks() + 6;
+			jacB = &data.m_jacobians[constraintRow.m_jacBindex];
+			lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
+			for (int i = 0; i < ndofB; ++i)
+			{
+				btScalar j = jacB[i] ;
+				btScalar l =lambdaB[i];
+				denom1 += j*l;
+			}
+
+		} 
+
+		 if (multiBodyA && (multiBodyA==multiBodyB))
+		 {
+            // ndof1 == ndof2 in this case
+            for (int i = 0; i < ndofA; ++i) 
+			{
+                denom1 += jacB[i] * lambdaA[i];
+                denom1 += jacA[i] * lambdaB[i];
+            }
+        }
+
+		 btScalar d = denom0+denom1;
+		 if (btFabs(d)>SIMD_EPSILON)
+		 {
+			 
+			 constraintRow.m_jacDiagABInv = 1.f/(d);
+		 } else
+		 {
+			constraintRow.m_jacDiagABInv  = 1.f;
+		 }
+		
+	}
+
+	
+	//compute rhs and remaining constraintRow fields
+
+	
+
+
+	btScalar rel_vel = 0.f;
+	int ndofA  = 0;
+	int ndofB  = 0;
+	{
+
+		btVector3 vel1,vel2;
+		if (multiBodyA)
+		{
+			ndofA  = multiBodyA->getNumLinks() + 6;
+			btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
+			for (int i = 0; i < ndofA ; ++i) 
+				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
+		} 
+		if (multiBodyB)
+		{
+			ndofB  = multiBodyB->getNumLinks() + 6;
+			btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
+			for (int i = 0; i < ndofB ; ++i) 
+				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
+
+		}
+
+		constraintRow.m_friction = 0.f;
+
+		constraintRow.m_appliedImpulse = 0.f;
+		constraintRow.m_appliedPushImpulse = 0.f;
+		
+		btScalar	velocityError =  desiredVelocity - rel_vel;// * damping;
+
+		btScalar erp = infoGlobal.m_erp2;
+
+		btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
+
+		if (!infoGlobal.m_splitImpulse)
+		{
+			//combine position and velocity into rhs
+			constraintRow.m_rhs = velocityImpulse;
+			constraintRow.m_rhsPenetration = 0.f;
+
+		} else
+		{
+			//split position and velocity into rhs and m_rhsPenetration
+			constraintRow.m_rhs = velocityImpulse;
+			constraintRow.m_rhsPenetration = 0.f;
+		}
+
+
+		constraintRow.m_cfm = 0.f;
+		constraintRow.m_lowerLimit = lowerLimit;
+		constraintRow.m_upperLimit = upperLimit;
+
+	}
+	return rel_vel;
+}
+
+
+void	btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
+{
+	for (int i = 0; i < ndof; ++i) 
+		data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
+}
+
+
+void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, 
+																	btMultiBodyJacobianData& data,
+																 const btVector3& contactNormalOnB,
+																 const btVector3& posAworld, const btVector3& posBworld, 
+																 btScalar position,
+																 const btContactSolverInfo& infoGlobal,
+																 btScalar& relaxation,
+																 bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+{
+			
+	
+	btVector3 rel_pos1 = posAworld;
+	btVector3 rel_pos2 = posBworld;
+
+	solverConstraint.m_multiBodyA = m_bodyA;
+	solverConstraint.m_multiBodyB = m_bodyB;
+	solverConstraint.m_linkA = m_linkA;
+	solverConstraint.m_linkB = m_linkB;
+	
+
+	btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
+	btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
+
+	const btVector3& pos1 = posAworld;
+	const btVector3& pos2 = posBworld;
+
+	btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
+	btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
+
+	btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
+	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
+
+	if (bodyA)
+		rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
+	if (bodyB)
+		rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
+
+	relaxation = 1.f;
+
+	if (multiBodyA)
+	{
+		const int ndofA  = multiBodyA->getNumLinks() + 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);
+		}
+
+		solverConstraint.m_jacAindex = data.m_jacobians.size();
+		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
+		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+
+		btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
+		multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+		multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+	} else
+	{
+		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
+		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+		solverConstraint.m_contactNormal1 = contactNormalOnB;
+	}
+
+	if (multiBodyB)
+	{
+		const int ndofB  = multiBodyB->getNumLinks() + 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);
+		}
+
+		solverConstraint.m_jacBindex = data.m_jacobians.size();
+
+		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
+		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+
+		multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+		multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
+	} else
+	{
+		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);		
+		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+		solverConstraint.m_contactNormal2 = -contactNormalOnB;
+	}
+
+	{
+						
+		btVector3 vec;
+		btScalar denom0 = 0.f;
+		btScalar denom1 = 0.f;
+		btScalar* jacB = 0;
+		btScalar* jacA = 0;
+		btScalar* lambdaA =0;
+		btScalar* lambdaB =0;
+		int ndofA  = 0;
+		if (multiBodyA)
+		{
+			ndofA  = multiBodyA->getNumLinks() + 6;
+			jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+			lambdaA = &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() + contactNormalOnB.dot(vec);
+			}
+		}
+		if (multiBodyB)
+		{
+			const int ndofB  = multiBodyB->getNumLinks() + 6;
+			jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+			lambdaB = &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() + contactNormalOnB.dot(vec);
+			}
+		}
+
+		 if (multiBodyA && (multiBodyA==multiBodyB))
+		 {
+            // ndof1 == ndof2 in this case
+            for (int i = 0; i < ndofA; ++i) 
+			{
+                denom1 += jacB[i] * lambdaA[i];
+                denom1 += jacA[i] * lambdaB[i];
+            }
+        }
+
+		 btScalar d = denom0+denom1;
+		 if (btFabs(d)>SIMD_EPSILON)
+		 {
+			 
+			 solverConstraint.m_jacDiagABInv = relaxation/(d);
+		 } else
+		 {
+			solverConstraint.m_jacDiagABInv  = 1.f;
+		 }
+		
+	}
+
+	
+	//compute rhs and remaining solverConstraint fields
+
+	
+
+	btScalar restitution = 0.f;
+	btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop;
+
+	btScalar rel_vel = 0.f;
+	int ndofA  = 0;
+	int ndofB  = 0;
+	{
+
+		btVector3 vel1,vel2;
+		if (multiBodyA)
+		{
+			ndofA  = multiBodyA->getNumLinks() + 6;
+			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->getNumLinks() + 6;
+			btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+			for (int i = 0; i < ndofB ; ++i) 
+				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
+
+		} else
+		{
+			if (rb1)
+			{
+				rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+			}
+		}
+
+		solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
+
+				
+		restitution =  restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
+		if (restitution <= btScalar(0.))
+		{
+			restitution = 0.f;
+		};
+	}
+
+
+	///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 = restitution - 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*solverConstraint.m_jacDiagABInv;
+		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+
+		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 = -m_maxAppliedImpulse;
+		solverConstraint.m_upperLimit = m_maxAppliedImpulse;
+	}
+
+}

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -0,0 +1,166 @@
+/*
+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;
+	btAlignedObjectArray<btScalar>		m_deltaVelocities;
+	btAlignedObjectArray<btScalar>		scratch_r;
+	btAlignedObjectArray<btVector3>		scratch_v;
+	btAlignedObjectArray<btMatrix3x3>	scratch_m;
+	btAlignedObjectArray<btSolverBody>*	m_solverBodyPool;
+	int									m_fixedBodyId;
+
+};
+
+
+class btMultiBodyConstraint
+{
+protected:
+
+	btMultiBody*	m_bodyA;
+    btMultiBody*	m_bodyB;
+    int				m_linkA;
+    int				m_linkB;
+
+    int				m_num_rows;
+    int				m_jac_size_A;
+    int				m_jac_size_both;
+    int				m_pos_offset;
+
+	bool			m_isUnilateral;
+
+	btScalar		m_maxAppliedImpulse;
+
+
+    // 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);
+
+	void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, 
+																	btMultiBodyJacobianData& data,
+																 const btVector3& contactNormalOnB,
+																 const btVector3& posAworld, const btVector3& posBworld, 
+																 btScalar position,
+																 const btContactSolverInfo& infoGlobal,
+																 btScalar& relaxation,
+																 bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
+
+		btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
+														btMultiBodyJacobianData& data,
+														btScalar* jacOrgA,btScalar* jacOrgB,
+														const btContactSolverInfo& infoGlobal,
+														btScalar desiredVelocity,
+														btScalar lowerLimit,
+														btScalar upperLimit);
+
+public:
+
+	btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
+	virtual ~btMultiBodyConstraint();
+
+
+
+	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_num_rows;
+	}
+
+	btMultiBody*	getMultiBodyA()
+	{
+		return m_bodyA;
+	}
+    btMultiBody*	getMultiBodyB()
+	{
+		return m_bodyB;
+	}
+
+	// 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_pos_offset + row]; 
+	}
+
+    void setPosition(int row, btScalar pos) 
+	{ 
+		m_data[m_pos_offset + 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_num_rows + row * m_jac_size_both]; 
+	}
+    const btScalar* jacobianA(int row) const 
+	{ 
+		return &m_data[m_num_rows + (row * m_jac_size_both)]; 
+	}
+    btScalar* jacobianB(int row) 
+	{ 
+		return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; 
+	}
+    const btScalar* jacobianB(int row) const 
+	{ 
+		return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; 
+	}
+
+	btScalar	getMaxAppliedImpulse() const
+	{
+		return m_maxAppliedImpulse;
+	}
+	void	setMaxAppliedImpulse(btScalar maxImp)
+	{
+		m_maxAppliedImpulse = maxImp;
+	}
+	
+
+};
+
+#endif //BT_MULTIBODY_CONSTRAINT_H
+

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -0,0 +1,795 @@
+/*
+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 val = 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++)
+	{
+		btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
+		//if (iteration < constraint.m_overrideNumSolverIterations)
+			//resolveSingleConstraintRowGenericMultiBody(constraint);
+		resolveSingleConstraintRowGeneric(constraint);
+	}
+
+	//solve featherstone normal contact
+	for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
+	{
+		btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
+		if (iteration < infoGlobal.m_numIterations)
+			resolveSingleConstraintRowGeneric(constraint);
+	}
+	
+	//solve featherstone frictional contact
+
+	for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++)
+	{
+		if (iteration < infoGlobal.m_numIterations)
+		{
+			btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j];
+			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;
+				resolveSingleConstraintRowGeneric(frictionConstraint);
+			}
+		}
+	}
+	return val;
+}
+
+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;
+}
+
+void 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->getNumLinks() + 6;
+		for (int i = 0; i < ndofA; ++i) 
+			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
+	} else
+	{
+		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->getNumLinks() + 6;
+		for (int i = 0; i < ndofB; ++i) 
+			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
+	} else
+	{
+		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);
+		c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
+	} else
+	{
+		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);
+		c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
+	} else
+	{
+		bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+	}
+
+}
+
+
+void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c)
+{
+
+	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
+	btScalar deltaVelADotn=0;
+	btScalar deltaVelBDotn=0;
+	int ndofA=0;
+	int ndofB=0;
+
+	if (c.m_multiBodyA)
+	{
+		ndofA  = c.m_multiBodyA->getNumLinks() + 6;
+		for (int i = 0; i < ndofA; ++i) 
+			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
+	}
+
+	if (c.m_multiBodyB)
+	{
+		ndofB = c.m_multiBodyB->getNumLinks() + 6;
+		for (int i = 0; i < ndofB; ++i) 
+			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
+	}
+
+	
+	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);
+		c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
+	}
+	if (c.m_multiBodyB)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
+		c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],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 = 1.f;
+
+	if (multiBodyA)
+	{
+		const int ndofA  = multiBodyA->getNumLinks() + 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->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+		btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+		multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
+	} else
+	{
+		btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
+		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+		solverConstraint.m_contactNormal1 = contactNormal;
+	}
+
+	if (multiBodyB)
+	{
+		const int ndofB  = multiBodyB->getNumLinks() + 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->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+		multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
+	} else
+	{
+		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);		
+		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+		solverConstraint.m_contactNormal2 = -contactNormal;
+	}
+
+	{
+						
+		btVector3 vec;
+		btScalar denom0 = 0.f;
+		btScalar denom1 = 0.f;
+		btScalar* jacB = 0;
+		btScalar* jacA = 0;
+		btScalar* lambdaA =0;
+		btScalar* lambdaB =0;
+		int ndofA  = 0;
+		if (multiBodyA)
+		{
+			ndofA  = multiBodyA->getNumLinks() + 6;
+			jacA = &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->getNumLinks() + 6;
+			jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
+			lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+			for (int i = 0; i < ndofB; ++i)
+			{
+				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);
+			}
+		}
+
+		 if (multiBodyA && (multiBodyA==multiBodyB))
+		 {
+            // ndof1 == ndof2 in this case
+            for (int i = 0; i < ndofA; ++i) 
+			{
+                denom1 += jacB[i] * lambdaA[i];
+                denom1 += jacA[i] * lambdaB[i];
+            }
+        }
+
+		 btScalar d = denom0+denom1;
+		 if (btFabs(d)>SIMD_EPSILON)
+		 {
+			 
+			 solverConstraint.m_jacDiagABInv = relaxation/(d);
+		 } else
+		 {
+			solverConstraint.m_jacDiagABInv  = 1.f;
+		 }
+		
+	}
+
+	
+	//compute rhs and remaining solverConstraint fields
+
+	
+
+	btScalar restitution = 0.f;
+	btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
+
+	btScalar rel_vel = 0.f;
+	int ndofA  = 0;
+	int ndofB  = 0;
+	{
+
+		btVector3 vel1,vel2;
+		if (multiBodyA)
+		{
+			ndofA  = multiBodyA->getNumLinks() + 6;
+			btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
+			for (int i = 0; i < ndofA ; ++i) 
+				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
+		} else
+		{
+			if (rb0)
+			{
+				rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+			}
+		}
+		if (multiBodyB)
+		{
+			ndofB  = multiBodyB->getNumLinks() + 6;
+			btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
+			for (int i = 0; i < ndofB ; ++i) 
+				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
+
+		} else
+		{
+			if (rb1)
+			{
+				rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+			}
+		}
+
+		solverConstraint.m_friction = cp.m_combinedFriction;
+
+				
+		restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
+		if (restitution <= btScalar(0.))
+		{
+			restitution = 0.f;
+		};
+	}
+
+
+	///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 = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+				multiBodyA->applyDeltaVee(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->applyDeltaVee(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;
+					
+
+		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*solverConstraint.m_jacDiagABInv;
+		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+
+		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 = 0;
+		solverConstraint.m_upperLimit = 1e10f;
+	}
+
+}
+
+
+
+
+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_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;
+}
+
+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;
+
+	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_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;
+#if ROLLING_FRICTION
+			btVector3 angVelA(0,0,0),angVelB(0,0,0);
+			if (rb0)
+				angVelA = rb0->getAngularVelocity();
+			if (rb1)
+				angVelB = rb1->getAngularVelocity();
+			btVector3 relAngVel = angVelB-angVelA;
+
+			if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
+			{
+				//only a single rollingFriction per manifold
+				rollingFriction--;
+				if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
+				{
+					relAngVel.normalize();
+					applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					if (relAngVel.length()>0.001)
+						addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+
+				} else
+				{
+					addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					btVector3 axis0,axis1;
+					btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
+					applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					if (axis0.length()>0.001)
+						addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					if (axis1.length()>0.001)
+						addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+		
+				}
+			}
+#endif //ROLLING_FRICTION
+
+			///Bullet has several options to set the friction directions
+			///By default, each contact has only a single friction direction that is recomputed automatically very frame 
+			///based on the relative linear velocity.
+			///If the relative velocity it 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
+			///
+			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+			{/*
+				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
+				*/
+				{
+					btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+
+					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);
+					}
+
+					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
+
+					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
+					{
+						cp.m_lateralFrictionInitialized = true;
+					}
+				}
+
+			} else
+			{
+				addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
+
+				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_contactCFM2);
+
+				//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);
+}
+
+
+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;
+	
+
+}

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h

@@ -0,0 +1,85 @@
+/*
+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"
+
+
+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;
+
+	void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
+	void resolveSingleConstraintRowGenericMultiBody(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);
+
+
+	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);
+
+	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);
+
+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 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
+

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -0,0 +1,578 @@
+/*
+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"
+
+	
+
+
+void	btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short 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;
+	
+		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::solveConstraints(btContactSolverInfo& solverInfo)
+{
+
+	btAlignedObjectArray<btScalar> scratch_r;
+	btAlignedObjectArray<btVector3> scratch_v;
+	btAlignedObjectArray<btMatrix3x3> scratch_m;
+
+
+	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);
+
+
+	{
+		BT_PROFILE("btMultiBody addForce and 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)
+			{
+				scratch_r.resize(bod->getNumLinks()+1);
+				scratch_v.resize(bod->getNumLinks()+1);
+				scratch_m.resize(bod->getNumLinks()+1);
+
+				bod->clearForcesAndTorques();
+				bod->addBaseForce(m_gravity * bod->getBaseMass());
+
+				for (int j = 0; j < bod->getNumLinks(); ++j) 
+				{
+					bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+				}
+
+				bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
+			}
+		}
+	}
+
+	m_solverMultiBodyIslandCallback->processConstraints();
+	
+	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
+
+}
+
+void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
+{
+	btDiscreteDynamicsWorld::integrateTransforms(timeStep);
+
+	{
+		BT_PROFILE("btMultiBody stepPositions");
+		//integrate and update the Featherstone hierarchies
+		btAlignedObjectArray<btQuaternion> world_to_local;
+		btAlignedObjectArray<btVector3> local_origin;
+
+		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 links
+				world_to_local.resize(nLinks+1);
+				local_origin.resize(nLinks+1);
+
+				bod->stepPositions(timeStep);
+
+	 
+
+				world_to_local[0] = bod->getWorldToBaseRot();
+				local_origin[0] = bod->getBasePos();
+
+				if (bod->getBaseCollider())
+				{
+					btVector3 posr = local_origin[0];
+					float pos[4]={posr.x(),posr.y(),posr.z(),1};
+					float 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]));
+
+					bod->getBaseCollider()->setWorldTransform(tr);
+
+				}
+      
+				for (int k=0;k<bod->getNumLinks();k++)
+				{
+					const int parent = bod->getParent(k);
+					world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1];
+					local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k)));
+				}
+
+
+				for (int m=0;m<bod->getNumLinks();m++)
+				{
+					btMultiBodyLinkCollider* col = bod->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};
+						float 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);
+					}
+				}
+			} else
+			{
+				bod->clearVelocities();
+			}
+		}
+	}
+}
+
+
+
+void	btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
+{
+	m_multiBodyConstraints.push_back(constraint);
+}
+
+void	btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
+{
+	m_multiBodyConstraints.remove(constraint);
+}

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -0,0 +1,56 @@
+/*
+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"
+
+
+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;
+
+	virtual void	calculateSimulationIslands();
+	virtual void	updateActivationState(btScalar timeStep);
+	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
+	virtual void	integrateTransforms(btScalar timeStep);
+public:
+
+	btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+	
+	virtual ~btMultiBodyDynamicsWorld ();
+
+	virtual void	addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
+
+	virtual void	removeMultiBody(btMultiBody* body);
+
+	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);
+
+	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
+};
+#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -0,0 +1,133 @@
+/*
+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,body,link,link,2,true),
+	m_lowerBound(lower),
+	m_upperBound(upper)
+{
+	// the data.m_jacobians never change, so may as well
+    // initialize them here
+        
+    // note: we rely on the fact that data.m_jacobians are
+    // always initialized to zero by the Constraint ctor
+
+    // row 0: the lower bound
+    jacobianA(0)[6 + link] = 1;
+
+    // row 1: the upper bound
+    jacobianB(1)[6 + link] = -1;
+}
+btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
+{
+}
+
+int btMultiBodyJointLimitConstraint::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 btMultiBodyJointLimitConstraint::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 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.
+    
+    // row 0: the lower bound
+    setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);
+
+    // row 1: the upper bound
+    setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
+
+	for (int row=0;row<getNumRows();row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		constraintRow.m_multiBodyA = m_bodyA;
+		constraintRow.m_multiBodyB = m_bodyB;
+		
+		btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,-m_maxAppliedImpulse,m_maxAppliedImpulse);
+		{
+			btScalar penetration = getPosition(row);
+			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;
+			}
+		}
+	}
+
+}
+	
+	
+	
+

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h

@@ -0,0 +1,44 @@
+/*
+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 int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+		btMultiBodyJacobianData& data,
+		const btContactSolverInfo& infoGlobal);
+	
+	
+};
+
+#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
+

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -0,0 +1,89 @@
+/*
+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,link,1,true),
+	m_desiredVelocity(desiredVelocity)	
+{
+	m_maxAppliedImpulse = maxMotorImpulse;
+	// the data.m_jacobians never change, so may as well
+    // initialize them here
+        
+    // note: we rely on the fact that data.m_jacobians are
+    // always initialized to zero by the Constraint ctor
+
+    // row 0: the lower bound
+    jacobianA(0)[6 + link] = 1;
+}
+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.
+    
+  
+
+	for (int row=0;row<getNumRows();row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		
+		btScalar penetration = 0;
+		fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,m_desiredVelocity,-m_maxAppliedImpulse,m_maxAppliedImpulse);
+	}
+
+}
+	

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h

@@ -0,0 +1,47 @@
+/*
+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;
+
+public:
+
+	btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
+	virtual ~btMultiBodyJointMotor();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+		btMultiBodyJacobianData& data,
+		const btContactSolverInfo& infoGlobal);
+	
+	
+};
+
+#endif //BT_MULTIBODY_JOINT_MOTOR_H
+

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -0,0 +1,110 @@
+/*
+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
+};
+//
+// Link struct
+//
+
+struct btMultibodyLink 
+{
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+    btScalar joint_pos;    // qi
+
+    btScalar mass;         // mass of link
+    btVector3 inertia;   // inertia of link (local frame; diagonal)
+
+    int parent;         // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
+
+    btQuaternion zero_rot_parent_to_this;    // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
+
+    // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
+    // for prismatic: axis_top = zero;
+    //                axis_bottom = unit vector along the joint axis.
+    // for revolute: axis_top = unit vector along the rotation axis (u);
+    //               axis_bottom = u cross d_vector.
+    btVector3 axis_top;
+    btVector3 axis_bottom;
+
+    btVector3 d_vector;   // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only.
+
+    // e_vector is constant, but depends on the joint type
+    // prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
+    // revolute: vector from parent's COM to the pivot point, in PARENT's frame.
+    btVector3 e_vector;
+
+    bool is_revolute;   // true = revolute, false = prismatic
+
+    btQuaternion cached_rot_parent_to_this;   // rotates vectors in parent frame to vectors in local frame
+    btVector3 cached_r_vector;                // vector from COM of parent to COM of this link, in local frame.
+
+    btVector3 applied_force;    // In WORLD frame
+    btVector3 applied_torque;   // In WORLD frame
+    btScalar joint_torque;
+
+	class btMultiBodyLinkCollider* m_collider;
+	int m_flags;
+
+    // ctor: set some sensible defaults
+	btMultibodyLink()
+		: joint_pos(0),
+			mass(1),
+			parent(-1),
+			zero_rot_parent_to_this(1, 0, 0, 0),
+			is_revolute(false),
+			cached_rot_parent_to_this(1, 0, 0, 0),
+			joint_torque(0),
+			m_collider(0),
+			m_flags(0)
+	{
+		inertia.setValue(1, 1, 1);
+		axis_top.setValue(0, 0, 0);
+		axis_bottom.setValue(1, 0, 0);
+		d_vector.setValue(0, 0, 0);
+		e_vector.setValue(0, 0, 0);
+		cached_r_vector.setValue(0, 0, 0);
+		applied_force.setValue( 0, 0, 0);
+		applied_torque.setValue(0, 0, 0);
+	}
+
+    // routine to update cached_rot_parent_to_this and cached_r_vector
+    void updateCache()
+	{
+		if (is_revolute) 
+		{
+			cached_rot_parent_to_this = btQuaternion(axis_top,-joint_pos) * zero_rot_parent_to_this;
+			cached_r_vector = d_vector + quatRotate(cached_rot_parent_to_this,e_vector);
+		} else 
+		{
+			// cached_rot_parent_to_this never changes, so no need to update
+			cached_r_vector = e_vector + joint_pos * axis_bottom;
+		}
+	}
+};
+
+
+#endif //BT_MULTIBODY_LINK_H

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h

@@ -0,0 +1,92 @@
+/*
+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_PARENT_COLLISION) && link.parent == other->m_link)
+				return false;
+		}
+		
+		if (other->m_link>=0)
+		{
+			const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
+			if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.parent == this->m_link)
+				return false;
+		}
+		return true;
+	}
+};
+
+#endif //BT_FEATHERSTONE_LINK_COLLIDER_H
+

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp

@@ -0,0 +1,143 @@
+/*
+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"
+
+btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
+	:btMultiBodyConstraint(body,0,link,-1,3,false),
+	m_rigidBodyA(0),
+	m_rigidBodyB(bodyB),
+	m_pivotInA(pivotInA),
+	m_pivotInB(pivotInB)
+{
+}
+
+btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
+	:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,3,false),
+	m_rigidBodyA(0),
+	m_rigidBodyB(0),
+	m_pivotInA(pivotInA),
+	m_pivotInB(pivotInB)
+{
+}
+
+
+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;
+	for (int i=0;i<3;i++)
+	{
+
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+
+		constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
+		constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
+		
+
+		btVector3 contactNormalOnB(0,0,0);
+		contactNormalOnB[i] = -1;
+
+		btScalar penetration = 0;
+
+		 // 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 position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
+		btScalar relaxation = 1.f;
+		fillMultiBodyConstraintMixed(constraintRow, data,
+																 contactNormalOnB,
+																 pivotAworld, pivotBworld, 
+																 position,
+																 infoGlobal,
+																 relaxation,
+																 false);
+		constraintRow.m_lowerLimit = -m_maxAppliedImpulse;
+		constraintRow.m_upperLimit = m_maxAppliedImpulse;
+
+	}
+}

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h

@@ -0,0 +1,60 @@
+/*
+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"
+
+class btMultiBodyPoint2Point : public btMultiBodyConstraint
+{
+protected:
+
+	btRigidBody*	m_rigidBodyA;
+	btRigidBody*	m_rigidBodyB;
+	btVector3		m_pivotInA;
+	btVector3		m_pivotInB;
+	
+
+public:
+
+	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 int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+		btMultiBodyJacobianData& data,
+		const btContactSolverInfo& infoGlobal);
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	void setPivotInB(const btVector3& pivotInB)
+	{
+		m_pivotInB = pivotInB;
+	}
+
+	
+};
+
+#endif //BT_MULTIBODY_POINT2POINT_H

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h

@@ -0,0 +1,82 @@
+/*
+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;
+#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();
+
+
+	int				m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
+	btVector3		m_relpos1CrossNormal;
+	btVector3		m_contactNormal1;
+	int				m_jacAindex;
+
+	int				m_deltaVelBindex;
+	btVector3		m_relpos2CrossNormal;
+	btVector3		m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
+	int				m_jacBindex;
+
+	btVector3		m_angularComponentA;
+	btVector3		m_angularComponentB;
+	
+	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;
+
+	enum		btSolverConstraintType
+	{
+		BT_SOLVER_CONTACT_1D = 0,
+		BT_SOLVER_FRICTION_1D
+	};
+};
+
+typedef btAlignedObjectArray<btMultiBodySolverConstraint>	btMultiBodyConstraintArray;
+
+#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp

@@ -0,0 +1,2079 @@
+/*************************************************************************
+*                                                                       *
+* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+* All rights reserved.  Email: [email protected]   Web: www.q12.org          *
+*                                                                       *
+* This library is free software; you can redistribute it and/or         *
+* modify it under the terms of EITHER:                                  *
+*   (1) The GNU Lesser General Public License as published by the Free  *
+*       Software Foundation; either version 2.1 of the License, or (at  *
+*       your option) any later version. The text of the GNU Lesser      *
+*       General Public License is included with this library in the     *
+*       file LICENSE.TXT.                                               *
+*   (2) The BSD-style license that is included with this library in     *
+*       the file LICENSE-BSD.TXT.                                       *
+*                                                                       *
+* This library is distributed in the hope that it will be useful,       *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+* LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+*                                                                       *
+*************************************************************************/
+
+/*
+
+
+THE ALGORITHM
+-------------
+
+solve A*x = b+w, with x and w subject to certain LCP conditions.
+each x(i),w(i) must lie on one of the three line segments in the following
+diagram. each line segment corresponds to one index set :
+
+     w(i)
+     /|\      |           :
+      |       |           :
+      |       |i in N     :
+  w>0 |       |state[i]=0 :
+      |       |           :
+      |       |           :  i in C
+  w=0 +       +-----------------------+
+      |                   :           |
+      |                   :           |
+  w<0 |                   :           |i in N
+      |                   :           |state[i]=1
+      |                   :           |
+      |                   :           |
+      +-------|-----------|-----------|----------> x(i)
+             lo           0           hi
+
+the Dantzig algorithm proceeds as follows:
+  for i=1:n
+    * if (x(i),w(i)) is not on the line, push x(i) and w(i) positive or
+      negative towards the line. as this is done, the other (x(j),w(j))
+      for j<i are constrained to be on the line. if any (x,w) reaches the
+      end of a line segment then it is switched between index sets.
+    * i is added to the appropriate index set depending on what line segment
+      it hits.
+
+we restrict lo(i) <= 0 and hi(i) >= 0. this makes the algorithm a bit
+simpler, because the starting point for x(i),w(i) is always on the dotted
+line x=0 and x will only ever increase in one direction, so it can only hit
+two out of the three line segments.
+
+
+NOTES
+-----
+
+this is an implementation of "lcp_dantzig2_ldlt.m" and "lcp_dantzig_lohi.m".
+the implementation is split into an LCP problem object (btLCP) and an LCP
+driver function. most optimization occurs in the btLCP object.
+
+a naive implementation of the algorithm requires either a lot of data motion
+or a lot of permutation-array lookup, because we are constantly re-ordering
+rows and columns. to avoid this and make a more optimized algorithm, a
+non-trivial data structure is used to represent the matrix A (this is
+implemented in the fast version of the btLCP object).
+
+during execution of this algorithm, some indexes in A are clamped (set C),
+some are non-clamped (set N), and some are "don't care" (where x=0).
+A,x,b,w (and other problem vectors) are permuted such that the clamped
+indexes are first, the unclamped indexes are next, and the don't-care
+indexes are last. this permutation is recorded in the array `p'.
+initially p = 0..n-1, and as the rows and columns of A,x,b,w are swapped,
+the corresponding elements of p are swapped.
+
+because the C and N elements are grouped together in the rows of A, we can do
+lots of work with a fast dot product function. if A,x,etc were not permuted
+and we only had a permutation array, then those dot products would be much
+slower as we would have a permutation array lookup in some inner loops.
+
+A is accessed through an array of row pointers, so that element (i,j) of the
+permuted matrix is A[i][j]. this makes row swapping fast. for column swapping
+we still have to actually move the data.
+
+during execution of this algorithm we maintain an L*D*L' factorization of
+the clamped submatrix of A (call it `AC') which is the top left nC*nC
+submatrix of A. there are two ways we could arrange the rows/columns in AC.
+
+(1) AC is always permuted such that L*D*L' = AC. this causes a problem
+when a row/column is removed from C, because then all the rows/columns of A
+between the deleted index and the end of C need to be rotated downward.
+this results in a lot of data motion and slows things down.
+(2) L*D*L' is actually a factorization of a *permutation* of AC (which is
+itself a permutation of the underlying A). this is what we do - the
+permutation is recorded in the vector C. call this permutation A[C,C].
+when a row/column is removed from C, all we have to do is swap two
+rows/columns and manipulate C.
+
+*/
+
+
+#include "btDantzigLCP.h"
+
+#include <string.h>//memcpy
+
+bool s_error = false;
+
+//***************************************************************************
+// code generation parameters
+
+
+#define btLCP_FAST		// use fast btLCP object
+
+// option 1 : matrix row pointers (less data copying)
+#define BTROWPTRS
+#define BTATYPE btScalar **
+#define BTAROW(i) (m_A[i])
+
+// option 2 : no matrix row pointers (slightly faster inner loops)
+//#define NOROWPTRS
+//#define BTATYPE btScalar *
+//#define BTAROW(i) (m_A+(i)*m_nskip)
+
+#define BTNUB_OPTIMIZATIONS
+
+
+
+/* solve L*X=B, with B containing 1 right hand sides.
+ * L is an n*n lower triangular matrix with ones on the diagonal.
+ * L is stored by rows and its leading dimension is lskip.
+ * B is an n*1 matrix that contains the right hand sides.
+ * B is stored by columns and its leading dimension is also lskip.
+ * B is overwritten with X.
+ * this processes blocks of 2*2.
+ * if this is in the factorizer source file, n must be a multiple of 2.
+ */
+
+static void btSolveL1_1 (const btScalar *L, btScalar *B, int n, int lskip1)
+{  
+  /* declare variables - Z matrix, p and q vectors, etc */
+  btScalar Z11,m11,Z21,m21,p1,q1,p2,*ex;
+  const btScalar *ell;
+  int i,j;
+  /* compute all 2 x 1 blocks of X */
+  for (i=0; i < n; i+=2) {
+    /* compute all 2 x 1 block of X, from rows i..i+2-1 */
+    /* set the Z matrix to 0 */
+    Z11=0;
+    Z21=0;
+    ell = L + i*lskip1;
+    ex = B;
+    /* the inner loop that computes outer products and adds them to Z */
+    for (j=i-2; j >= 0; j -= 2) {
+      /* compute outer product and add it to the Z matrix */
+      p1=ell[0];
+      q1=ex[0];
+      m11 = p1 * q1;
+      p2=ell[lskip1];
+      m21 = p2 * q1;
+      Z11 += m11;
+      Z21 += m21;
+      /* compute outer product and add it to the Z matrix */
+      p1=ell[1];
+      q1=ex[1];
+      m11 = p1 * q1;
+      p2=ell[1+lskip1];
+      m21 = p2 * q1;
+      /* advance pointers */
+      ell += 2;
+      ex += 2;
+      Z11 += m11;
+      Z21 += m21;
+      /* end of inner loop */
+    }
+    /* compute left-over iterations */
+    j += 2;
+    for (; j > 0; j--) {
+      /* compute outer product and add it to the Z matrix */
+      p1=ell[0];
+      q1=ex[0];
+      m11 = p1 * q1;
+      p2=ell[lskip1];
+      m21 = p2 * q1;
+      /* advance pointers */
+      ell += 1;
+      ex += 1;
+      Z11 += m11;
+      Z21 += m21;
+    }
+    /* finish computing the X(i) block */
+    Z11 = ex[0] - Z11;
+    ex[0] = Z11;
+    p1 = ell[lskip1];
+    Z21 = ex[1] - Z21 - p1*Z11;
+    ex[1] = Z21;
+    /* end of outer loop */
+  }
+}
+
+/* solve L*X=B, with B containing 2 right hand sides.
+ * L is an n*n lower triangular matrix with ones on the diagonal.
+ * L is stored by rows and its leading dimension is lskip.
+ * B is an n*2 matrix that contains the right hand sides.
+ * B is stored by columns and its leading dimension is also lskip.
+ * B is overwritten with X.
+ * this processes blocks of 2*2.
+ * if this is in the factorizer source file, n must be a multiple of 2.
+ */
+
+static void btSolveL1_2 (const btScalar *L, btScalar *B, int n, int lskip1)
+{  
+  /* declare variables - Z matrix, p and q vectors, etc */
+  btScalar Z11,m11,Z12,m12,Z21,m21,Z22,m22,p1,q1,p2,q2,*ex;
+  const btScalar *ell;
+  int i,j;
+  /* compute all 2 x 2 blocks of X */
+  for (i=0; i < n; i+=2) {
+    /* compute all 2 x 2 block of X, from rows i..i+2-1 */
+    /* set the Z matrix to 0 */
+    Z11=0;
+    Z12=0;
+    Z21=0;
+    Z22=0;
+    ell = L + i*lskip1;
+    ex = B;
+    /* the inner loop that computes outer products and adds them to Z */
+    for (j=i-2; j >= 0; j -= 2) {
+      /* compute outer product and add it to the Z matrix */
+      p1=ell[0];
+      q1=ex[0];
+      m11 = p1 * q1;
+      q2=ex[lskip1];
+      m12 = p1 * q2;
+      p2=ell[lskip1];
+      m21 = p2 * q1;
+      m22 = p2 * q2;
+      Z11 += m11;
+      Z12 += m12;
+      Z21 += m21;
+      Z22 += m22;
+      /* compute outer product and add it to the Z matrix */
+      p1=ell[1];
+      q1=ex[1];
+      m11 = p1 * q1;
+      q2=ex[1+lskip1];
+      m12 = p1 * q2;
+      p2=ell[1+lskip1];
+      m21 = p2 * q1;
+      m22 = p2 * q2;
+      /* advance pointers */
+      ell += 2;
+      ex += 2;
+      Z11 += m11;
+      Z12 += m12;
+      Z21 += m21;
+      Z22 += m22;
+      /* end of inner loop */
+    }
+    /* compute left-over iterations */
+    j += 2;
+    for (; j > 0; j--) {
+      /* compute outer product and add it to the Z matrix */
+      p1=ell[0];
+      q1=ex[0];
+      m11 = p1 * q1;
+      q2=ex[lskip1];
+      m12 = p1 * q2;
+      p2=ell[lskip1];
+      m21 = p2 * q1;
+      m22 = p2 * q2;
+      /* advance pointers */
+      ell += 1;
+      ex += 1;
+      Z11 += m11;
+      Z12 += m12;
+      Z21 += m21;
+      Z22 += m22;
+    }
+    /* finish computing the X(i) block */
+    Z11 = ex[0] - Z11;
+    ex[0] = Z11;
+    Z12 = ex[lskip1] - Z12;
+    ex[lskip1] = Z12;
+    p1 = ell[lskip1];
+    Z21 = ex[1] - Z21 - p1*Z11;
+    ex[1] = Z21;
+    Z22 = ex[1+lskip1] - Z22 - p1*Z12;
+    ex[1+lskip1] = Z22;
+    /* end of outer loop */
+  }
+}
+
+
+void btFactorLDLT (btScalar *A, btScalar *d, int n, int nskip1)
+{  
+  int i,j;
+  btScalar sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22;
+  if (n < 1) return;
+  
+  for (i=0; i<=n-2; i += 2) {
+    /* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */
+    btSolveL1_2 (A,A+i*nskip1,i,nskip1);
+    /* scale the elements in a 2 x i block at A(i,0), and also */
+    /* compute Z = the outer product matrix that we'll need. */
+    Z11 = 0;
+    Z21 = 0;
+    Z22 = 0;
+    ell = A+i*nskip1;
+    dee = d;
+    for (j=i-6; j >= 0; j -= 6) {
+      p1 = ell[0];
+      p2 = ell[nskip1];
+      dd = dee[0];
+      q1 = p1*dd;
+      q2 = p2*dd;
+      ell[0] = q1;
+      ell[nskip1] = q2;
+      m11 = p1*q1;
+      m21 = p2*q1;
+      m22 = p2*q2;
+      Z11 += m11;
+      Z21 += m21;
+      Z22 += m22;
+      p1 = ell[1];
+      p2 = ell[1+nskip1];
+      dd = dee[1];
+      q1 = p1*dd;
+      q2 = p2*dd;
+      ell[1] = q1;
+      ell[1+nskip1] = q2;
+      m11 = p1*q1;
+      m21 = p2*q1;
+      m22 = p2*q2;
+      Z11 += m11;
+      Z21 += m21;
+      Z22 += m22;
+      p1 = ell[2];
+      p2 = ell[2+nskip1];
+      dd = dee[2];
+      q1 = p1*dd;
+      q2 = p2*dd;
+      ell[2] = q1;
+      ell[2+nskip1] = q2;
+      m11 = p1*q1;
+      m21 = p2*q1;
+      m22 = p2*q2;
+      Z11 += m11;
+      Z21 += m21;
+      Z22 += m22;
+      p1 = ell[3];
+      p2 = ell[3+nskip1];
+      dd = dee[3];
+      q1 = p1*dd;
+      q2 = p2*dd;
+      ell[3] = q1;
+      ell[3+nskip1] = q2;
+      m11 = p1*q1;
+      m21 = p2*q1;
+      m22 = p2*q2;
+      Z11 += m11;
+      Z21 += m21;
+      Z22 += m22;
+      p1 = ell[4];
+      p2 = ell[4+nskip1];
+      dd = dee[4];
+      q1 = p1*dd;
+      q2 = p2*dd;
+      ell[4] = q1;
+      ell[4+nskip1] = q2;
+      m11 = p1*q1;
+      m21 = p2*q1;
+      m22 = p2*q2;
+      Z11 += m11;
+      Z21 += m21;
+      Z22 += m22;
+      p1 = ell[5];
+      p2 = ell[5+nskip1];
+      dd = dee[5];
+      q1 = p1*dd;
+      q2 = p2*dd;
+      ell[5] = q1;
+      ell[5+nskip1] = q2;
+      m11 = p1*q1;
+      m21 = p2*q1;
+      m22 = p2*q2;
+      Z11 += m11;
+      Z21 += m21;
+      Z22 += m22;
+      ell += 6;
+      dee += 6;
+    }
+    /* compute left-over iterations */
+    j += 6;
+    for (; j > 0; j--) {
+      p1 = ell[0];
+      p2 = ell[nskip1];
+      dd = dee[0];
+      q1 = p1*dd;
+      q2 = p2*dd;
+      ell[0] = q1;
+      ell[nskip1] = q2;
+      m11 = p1*q1;
+      m21 = p2*q1;
+      m22 = p2*q2;
+      Z11 += m11;
+      Z21 += m21;
+      Z22 += m22;
+      ell++;
+      dee++;
+    }
+    /* solve for diagonal 2 x 2 block at A(i,i) */
+    Z11 = ell[0] - Z11;
+    Z21 = ell[nskip1] - Z21;
+    Z22 = ell[1+nskip1] - Z22;
+    dee = d + i;
+    /* factorize 2 x 2 block Z,dee */
+    /* factorize row 1 */
+    dee[0] = btRecip(Z11);
+    /* factorize row 2 */
+    sum = 0;
+    q1 = Z21;
+    q2 = q1 * dee[0];
+    Z21 = q2;
+    sum += q1*q2;
+    dee[1] = btRecip(Z22 - sum);
+    /* done factorizing 2 x 2 block */
+    ell[nskip1] = Z21;
+  }
+  /* compute the (less than 2) rows at the bottom */
+  switch (n-i) {
+    case 0:
+    break;
+    
+    case 1:
+    btSolveL1_1 (A,A+i*nskip1,i,nskip1);
+    /* scale the elements in a 1 x i block at A(i,0), and also */
+    /* compute Z = the outer product matrix that we'll need. */
+    Z11 = 0;
+    ell = A+i*nskip1;
+    dee = d;
+    for (j=i-6; j >= 0; j -= 6) {
+      p1 = ell[0];
+      dd = dee[0];
+      q1 = p1*dd;
+      ell[0] = q1;
+      m11 = p1*q1;
+      Z11 += m11;
+      p1 = ell[1];
+      dd = dee[1];
+      q1 = p1*dd;
+      ell[1] = q1;
+      m11 = p1*q1;
+      Z11 += m11;
+      p1 = ell[2];
+      dd = dee[2];
+      q1 = p1*dd;
+      ell[2] = q1;
+      m11 = p1*q1;
+      Z11 += m11;
+      p1 = ell[3];
+      dd = dee[3];
+      q1 = p1*dd;
+      ell[3] = q1;
+      m11 = p1*q1;
+      Z11 += m11;
+      p1 = ell[4];
+      dd = dee[4];
+      q1 = p1*dd;
+      ell[4] = q1;
+      m11 = p1*q1;
+      Z11 += m11;
+      p1 = ell[5];
+      dd = dee[5];
+      q1 = p1*dd;
+      ell[5] = q1;
+      m11 = p1*q1;
+      Z11 += m11;
+      ell += 6;
+      dee += 6;
+    }
+    /* compute left-over iterations */
+    j += 6;
+    for (; j > 0; j--) {
+      p1 = ell[0];
+      dd = dee[0];
+      q1 = p1*dd;
+      ell[0] = q1;
+      m11 = p1*q1;
+      Z11 += m11;
+      ell++;
+      dee++;
+    }
+    /* solve for diagonal 1 x 1 block at A(i,i) */
+    Z11 = ell[0] - Z11;
+    dee = d + i;
+    /* factorize 1 x 1 block Z,dee */
+    /* factorize row 1 */
+    dee[0] = btRecip(Z11);
+    /* done factorizing 1 x 1 block */
+    break;
+    
+    //default: *((char*)0)=0;  /* this should never happen! */
+  }
+}
+
+/* solve L*X=B, with B containing 1 right hand sides.
+ * L is an n*n lower triangular matrix with ones on the diagonal.
+ * L is stored by rows and its leading dimension is lskip.
+ * B is an n*1 matrix that contains the right hand sides.
+ * B is stored by columns and its leading dimension is also lskip.
+ * B is overwritten with X.
+ * this processes blocks of 4*4.
+ * if this is in the factorizer source file, n must be a multiple of 4.
+ */
+
+void btSolveL1 (const btScalar *L, btScalar *B, int n, int lskip1)
+{  
+  /* declare variables - Z matrix, p and q vectors, etc */
+  btScalar Z11,Z21,Z31,Z41,p1,q1,p2,p3,p4,*ex;
+  const btScalar *ell;
+  int lskip2,lskip3,i,j;
+  /* compute lskip values */
+  lskip2 = 2*lskip1;
+  lskip3 = 3*lskip1;
+  /* compute all 4 x 1 blocks of X */
+  for (i=0; i <= n-4; i+=4) {
+    /* compute all 4 x 1 block of X, from rows i..i+4-1 */
+    /* set the Z matrix to 0 */
+    Z11=0;
+    Z21=0;
+    Z31=0;
+    Z41=0;
+    ell = L + i*lskip1;
+    ex = B;
+    /* the inner loop that computes outer products and adds them to Z */
+    for (j=i-12; j >= 0; j -= 12) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      p2=ell[lskip1];
+      p3=ell[lskip2];
+      p4=ell[lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[1];
+      q1=ex[1];
+      p2=ell[1+lskip1];
+      p3=ell[1+lskip2];
+      p4=ell[1+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[2];
+      q1=ex[2];
+      p2=ell[2+lskip1];
+      p3=ell[2+lskip2];
+      p4=ell[2+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[3];
+      q1=ex[3];
+      p2=ell[3+lskip1];
+      p3=ell[3+lskip2];
+      p4=ell[3+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[4];
+      q1=ex[4];
+      p2=ell[4+lskip1];
+      p3=ell[4+lskip2];
+      p4=ell[4+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[5];
+      q1=ex[5];
+      p2=ell[5+lskip1];
+      p3=ell[5+lskip2];
+      p4=ell[5+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[6];
+      q1=ex[6];
+      p2=ell[6+lskip1];
+      p3=ell[6+lskip2];
+      p4=ell[6+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[7];
+      q1=ex[7];
+      p2=ell[7+lskip1];
+      p3=ell[7+lskip2];
+      p4=ell[7+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[8];
+      q1=ex[8];
+      p2=ell[8+lskip1];
+      p3=ell[8+lskip2];
+      p4=ell[8+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[9];
+      q1=ex[9];
+      p2=ell[9+lskip1];
+      p3=ell[9+lskip2];
+      p4=ell[9+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[10];
+      q1=ex[10];
+      p2=ell[10+lskip1];
+      p3=ell[10+lskip2];
+      p4=ell[10+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[11];
+      q1=ex[11];
+      p2=ell[11+lskip1];
+      p3=ell[11+lskip2];
+      p4=ell[11+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* advance pointers */
+      ell += 12;
+      ex += 12;
+      /* end of inner loop */
+    }
+    /* compute left-over iterations */
+    j += 12;
+    for (; j > 0; j--) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      p2=ell[lskip1];
+      p3=ell[lskip2];
+      p4=ell[lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* advance pointers */
+      ell += 1;
+      ex += 1;
+    }
+    /* finish computing the X(i) block */
+    Z11 = ex[0] - Z11;
+    ex[0] = Z11;
+    p1 = ell[lskip1];
+    Z21 = ex[1] - Z21 - p1*Z11;
+    ex[1] = Z21;
+    p1 = ell[lskip2];
+    p2 = ell[1+lskip2];
+    Z31 = ex[2] - Z31 - p1*Z11 - p2*Z21;
+    ex[2] = Z31;
+    p1 = ell[lskip3];
+    p2 = ell[1+lskip3];
+    p3 = ell[2+lskip3];
+    Z41 = ex[3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
+    ex[3] = Z41;
+    /* end of outer loop */
+  }
+  /* compute rows at end that are not a multiple of block size */
+  for (; i < n; i++) {
+    /* compute all 1 x 1 block of X, from rows i..i+1-1 */
+    /* set the Z matrix to 0 */
+    Z11=0;
+    ell = L + i*lskip1;
+    ex = B;
+    /* the inner loop that computes outer products and adds them to Z */
+    for (j=i-12; j >= 0; j -= 12) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[1];
+      q1=ex[1];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[2];
+      q1=ex[2];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[3];
+      q1=ex[3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[4];
+      q1=ex[4];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[5];
+      q1=ex[5];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[6];
+      q1=ex[6];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[7];
+      q1=ex[7];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[8];
+      q1=ex[8];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[9];
+      q1=ex[9];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[10];
+      q1=ex[10];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[11];
+      q1=ex[11];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* advance pointers */
+      ell += 12;
+      ex += 12;
+      /* end of inner loop */
+    }
+    /* compute left-over iterations */
+    j += 12;
+    for (; j > 0; j--) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* advance pointers */
+      ell += 1;
+      ex += 1;
+    }
+    /* finish computing the X(i) block */
+    Z11 = ex[0] - Z11;
+    ex[0] = Z11;
+  }
+}
+
+/* solve L^T * x=b, with b containing 1 right hand side.
+ * L is an n*n lower triangular matrix with ones on the diagonal.
+ * L is stored by rows and its leading dimension is lskip.
+ * b is an n*1 matrix that contains the right hand side.
+ * b is overwritten with x.
+ * this processes blocks of 4.
+ */
+
+void btSolveL1T (const btScalar *L, btScalar *B, int n, int lskip1)
+{  
+  /* declare variables - Z matrix, p and q vectors, etc */
+  btScalar Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex;
+  const btScalar *ell;
+  int lskip2,lskip3,i,j;
+  /* special handling for L and B because we're solving L1 *transpose* */
+  L = L + (n-1)*(lskip1+1);
+  B = B + n-1;
+  lskip1 = -lskip1;
+  /* compute lskip values */
+  lskip2 = 2*lskip1;
+  lskip3 = 3*lskip1;
+  /* compute all 4 x 1 blocks of X */
+  for (i=0; i <= n-4; i+=4) {
+    /* compute all 4 x 1 block of X, from rows i..i+4-1 */
+    /* set the Z matrix to 0 */
+    Z11=0;
+    Z21=0;
+    Z31=0;
+    Z41=0;
+    ell = L - i;
+    ex = B;
+    /* the inner loop that computes outer products and adds them to Z */
+    for (j=i-4; j >= 0; j -= 4) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      p2=ell[-1];
+      p3=ell[-2];
+      p4=ell[-3];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      m21 = p2 * q1;
+      m31 = p3 * q1;
+      m41 = p4 * q1;
+      ell += lskip1;
+      Z11 += m11;
+      Z21 += m21;
+      Z31 += m31;
+      Z41 += m41;
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[-1];
+      p2=ell[-1];
+      p3=ell[-2];
+      p4=ell[-3];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      m21 = p2 * q1;
+      m31 = p3 * q1;
+      m41 = p4 * q1;
+      ell += lskip1;
+      Z11 += m11;
+      Z21 += m21;
+      Z31 += m31;
+      Z41 += m41;
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[-2];
+      p2=ell[-1];
+      p3=ell[-2];
+      p4=ell[-3];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      m21 = p2 * q1;
+      m31 = p3 * q1;
+      m41 = p4 * q1;
+      ell += lskip1;
+      Z11 += m11;
+      Z21 += m21;
+      Z31 += m31;
+      Z41 += m41;
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[-3];
+      p2=ell[-1];
+      p3=ell[-2];
+      p4=ell[-3];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      m21 = p2 * q1;
+      m31 = p3 * q1;
+      m41 = p4 * q1;
+      ell += lskip1;
+      ex -= 4;
+      Z11 += m11;
+      Z21 += m21;
+      Z31 += m31;
+      Z41 += m41;
+      /* end of inner loop */
+    }
+    /* compute left-over iterations */
+    j += 4;
+    for (; j > 0; j--) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      p2=ell[-1];
+      p3=ell[-2];
+      p4=ell[-3];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      m21 = p2 * q1;
+      m31 = p3 * q1;
+      m41 = p4 * q1;
+      ell += lskip1;
+      ex -= 1;
+      Z11 += m11;
+      Z21 += m21;
+      Z31 += m31;
+      Z41 += m41;
+    }
+    /* finish computing the X(i) block */
+    Z11 = ex[0] - Z11;
+    ex[0] = Z11;
+    p1 = ell[-1];
+    Z21 = ex[-1] - Z21 - p1*Z11;
+    ex[-1] = Z21;
+    p1 = ell[-2];
+    p2 = ell[-2+lskip1];
+    Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21;
+    ex[-2] = Z31;
+    p1 = ell[-3];
+    p2 = ell[-3+lskip1];
+    p3 = ell[-3+lskip2];
+    Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
+    ex[-3] = Z41;
+    /* end of outer loop */
+  }
+  /* compute rows at end that are not a multiple of block size */
+  for (; i < n; i++) {
+    /* compute all 1 x 1 block of X, from rows i..i+1-1 */
+    /* set the Z matrix to 0 */
+    Z11=0;
+    ell = L - i;
+    ex = B;
+    /* the inner loop that computes outer products and adds them to Z */
+    for (j=i-4; j >= 0; j -= 4) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      ell += lskip1;
+      Z11 += m11;
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[-1];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      ell += lskip1;
+      Z11 += m11;
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[-2];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      ell += lskip1;
+      Z11 += m11;
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[-3];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      ell += lskip1;
+      ex -= 4;
+      Z11 += m11;
+      /* end of inner loop */
+    }
+    /* compute left-over iterations */
+    j += 4;
+    for (; j > 0; j--) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      ell += lskip1;
+      ex -= 1;
+      Z11 += m11;
+    }
+    /* finish computing the X(i) block */
+    Z11 = ex[0] - Z11;
+    ex[0] = Z11;
+  }
+}
+
+
+
+void btVectorScale (btScalar *a, const btScalar *d, int n)
+{
+  btAssert (a && d && n >= 0);
+  for (int i=0; i<n; i++) {
+    a[i] *= d[i];
+  }
+}
+
+void btSolveLDLT (const btScalar *L, const btScalar *d, btScalar *b, int n, int nskip)
+{
+  btAssert (L && d && b && n > 0 && nskip >= n);
+  btSolveL1 (L,b,n,nskip);
+  btVectorScale (b,d,n);
+  btSolveL1T (L,b,n,nskip);
+}
+
+
+
+//***************************************************************************
+
+// swap row/column i1 with i2 in the n*n matrix A. the leading dimension of
+// A is nskip. this only references and swaps the lower triangle.
+// if `do_fast_row_swaps' is nonzero and row pointers are being used, then
+// rows will be swapped by exchanging row pointers. otherwise the data will
+// be copied.
+
+static void btSwapRowsAndCols (BTATYPE A, int n, int i1, int i2, int nskip, 
+  int do_fast_row_swaps)
+{
+  btAssert (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n &&
+    nskip >= n && i1 < i2);
+
+# ifdef BTROWPTRS
+  btScalar *A_i1 = A[i1];
+  btScalar *A_i2 = A[i2];
+  for (int i=i1+1; i<i2; ++i) {
+    btScalar *A_i_i1 = A[i] + i1;
+    A_i1[i] = *A_i_i1;
+    *A_i_i1 = A_i2[i];
+  }
+  A_i1[i2] = A_i1[i1];
+  A_i1[i1] = A_i2[i1];
+  A_i2[i1] = A_i2[i2];
+  // swap rows, by swapping row pointers
+  if (do_fast_row_swaps) {
+    A[i1] = A_i2;
+    A[i2] = A_i1;
+  }
+  else {
+    // Only swap till i2 column to match A plain storage variant.
+    for (int k = 0; k <= i2; ++k) {
+      btScalar tmp = A_i1[k];
+      A_i1[k] = A_i2[k];
+      A_i2[k] = tmp;
+    }
+  }
+  // swap columns the hard way
+  for (int j=i2+1; j<n; ++j) {
+    btScalar *A_j = A[j];
+    btScalar tmp = A_j[i1];
+    A_j[i1] = A_j[i2];
+    A_j[i2] = tmp;
+  }
+# else
+  btScalar *A_i1 = A+i1*nskip;
+  btScalar *A_i2 = A+i2*nskip;
+  for (int k = 0; k < i1; ++k) {
+    btScalar tmp = A_i1[k];
+    A_i1[k] = A_i2[k];
+    A_i2[k] = tmp;
+  }
+  btScalar *A_i = A_i1 + nskip;
+  for (int i=i1+1; i<i2; A_i+=nskip, ++i) {
+    btScalar tmp = A_i2[i];
+    A_i2[i] = A_i[i1];
+    A_i[i1] = tmp;
+  }
+  {
+    btScalar tmp = A_i1[i1];
+    A_i1[i1] = A_i2[i2];
+    A_i2[i2] = tmp;
+  }
+  btScalar *A_j = A_i2 + nskip;
+  for (int j=i2+1; j<n; A_j+=nskip, ++j) {
+    btScalar tmp = A_j[i1];
+    A_j[i1] = A_j[i2];
+    A_j[i2] = tmp;
+  }
+# endif
+}
+
+
+// swap two indexes in the n*n LCP problem. i1 must be <= i2.
+
+static void btSwapProblem (BTATYPE A, btScalar *x, btScalar *b, btScalar *w, btScalar *lo,
+                         btScalar *hi, int *p, bool *state, int *findex,
+                         int n, int i1, int i2, int nskip,
+                         int do_fast_row_swaps)
+{
+  btScalar tmpr;
+  int tmpi;
+  bool tmpb;
+  btAssert (n>0 && i1 >=0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && i1 <= i2);
+  if (i1==i2) return;
+  
+  btSwapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps);
+  
+  tmpr = x[i1];
+  x[i1] = x[i2];
+  x[i2] = tmpr;
+  
+  tmpr = b[i1];
+  b[i1] = b[i2];
+  b[i2] = tmpr;
+  
+  tmpr = w[i1];
+  w[i1] = w[i2];
+  w[i2] = tmpr;
+  
+  tmpr = lo[i1];
+  lo[i1] = lo[i2];
+  lo[i2] = tmpr;
+
+  tmpr = hi[i1];
+  hi[i1] = hi[i2];
+  hi[i2] = tmpr;
+
+  tmpi = p[i1];
+  p[i1] = p[i2];
+  p[i2] = tmpi;
+
+  tmpb = state[i1];
+  state[i1] = state[i2];
+  state[i2] = tmpb;
+
+  if (findex) {
+    tmpi = findex[i1];
+    findex[i1] = findex[i2];
+    findex[i2] = tmpi;
+  }
+}
+
+
+
+
+//***************************************************************************
+// btLCP manipulator object. this represents an n*n LCP problem.
+//
+// two index sets C and N are kept. each set holds a subset of
+// the variable indexes 0..n-1. an index can only be in one set.
+// initially both sets are empty.
+//
+// the index set C is special: solutions to A(C,C)\A(C,i) can be generated.
+
+//***************************************************************************
+// fast implementation of btLCP. see the above definition of btLCP for
+// interface comments.
+//
+// `p' records the permutation of A,x,b,w,etc. p is initially 1:n and is
+// permuted as the other vectors/matrices are permuted.
+//
+// A,x,b,w,lo,hi,state,findex,p,c are permuted such that sets C,N have
+// contiguous indexes. the don't-care indexes follow N.
+//
+// an L*D*L' factorization is maintained of A(C,C), and whenever indexes are
+// added or removed from the set C the factorization is updated.
+// thus L*D*L'=A[C,C], i.e. a permuted top left nC*nC submatrix of A.
+// the leading dimension of the matrix L is always `nskip'.
+//
+// at the start there may be other indexes that are unbounded but are not
+// included in `nub'. btLCP will permute the matrix so that absolutely all
+// unbounded vectors are at the start. thus there may be some initial
+// permutation.
+//
+// the algorithms here assume certain patterns, particularly with respect to
+// index transfer.
+
+#ifdef btLCP_FAST
+
+struct btLCP 
+{
+	const int m_n;
+	const int m_nskip;
+	int m_nub;
+	int m_nC, m_nN;				// size of each index set
+	BTATYPE const m_A;				// A rows
+	btScalar *const m_x, * const m_b, *const m_w, *const m_lo,* const m_hi;	// permuted LCP problem data
+	btScalar *const m_L, *const m_d;				// L*D*L' factorization of set C
+	btScalar *const m_Dell, *const m_ell, *const m_tmp;
+	bool *const m_state;
+	int *const m_findex, *const m_p, *const m_C;
+
+	btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
+		btScalar *_lo, btScalar *_hi, btScalar *_L, btScalar *_d,
+		btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
+		bool *_state, int *_findex, int *_p, int *_C, btScalar **Arows);
+	int getNub() const { return m_nub; }
+	void transfer_i_to_C (int i);
+	void transfer_i_to_N (int i) { m_nN++; }			// because we can assume C and N span 1:i-1
+	void transfer_i_from_N_to_C (int i);
+	void transfer_i_from_C_to_N (int i, btAlignedObjectArray<btScalar>& scratch);
+	int numC() const { return m_nC; }
+	int numN() const { return m_nN; }
+	int indexC (int i) const { return i; }
+	int indexN (int i) const { return i+m_nC; }
+	btScalar Aii (int i) const  { return BTAROW(i)[i]; }
+	btScalar AiC_times_qC (int i, btScalar *q) const { return btLargeDot (BTAROW(i), q, m_nC); }
+	btScalar AiN_times_qN (int i, btScalar *q) const { return btLargeDot (BTAROW(i)+m_nC, q+m_nC, m_nN); }
+	void pN_equals_ANC_times_qC (btScalar *p, btScalar *q);
+	void pN_plusequals_ANi (btScalar *p, int i, int sign=1);
+	void pC_plusequals_s_times_qC (btScalar *p, btScalar s, btScalar *q);
+	void pN_plusequals_s_times_qN (btScalar *p, btScalar s, btScalar *q);
+	void solve1 (btScalar *a, int i, int dir=1, int only_transfer=0);
+	void unpermute();
+};
+
+
+btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
+            btScalar *_lo, btScalar *_hi, btScalar *_L, btScalar *_d,
+            btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
+            bool *_state, int *_findex, int *_p, int *_C, btScalar **Arows):
+  m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0),
+# ifdef BTROWPTRS
+  m_A(Arows),
+#else
+  m_A(_Adata),
+#endif
+  m_x(_x), m_b(_b), m_w(_w), m_lo(_lo), m_hi(_hi),
+  m_L(_L), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp),
+  m_state(_state), m_findex(_findex), m_p(_p), m_C(_C)
+{
+  {
+    btSetZero (m_x,m_n);
+  }
+
+  {
+# ifdef BTROWPTRS
+    // make matrix row pointers
+    btScalar *aptr = _Adata;
+    BTATYPE A = m_A;
+    const int n = m_n, nskip = m_nskip;
+    for (int k=0; k<n; aptr+=nskip, ++k) A[k] = aptr;
+# endif
+  }
+
+  {
+    int *p = m_p;
+    const int n = m_n;
+    for (int k=0; k<n; ++k) p[k]=k;		// initially unpermuted
+  }
+
+  /*
+  // for testing, we can do some random swaps in the area i > nub
+  {
+    const int n = m_n;
+    const int nub = m_nub;
+    if (nub < n) {
+    for (int k=0; k<100; k++) {
+      int i1,i2;
+      do {
+        i1 = dRandInt(n-nub)+nub;
+        i2 = dRandInt(n-nub)+nub;
+      }
+      while (i1 > i2); 
+      //printf ("--> %d %d\n",i1,i2);
+      btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,n,i1,i2,m_nskip,0);
+    }
+  }
+  */
+
+  // permute the problem so that *all* the unbounded variables are at the
+  // start, i.e. look for unbounded variables not included in `nub'. we can
+  // potentially push up `nub' this way and get a bigger initial factorization.
+  // note that when we swap rows/cols here we must not just swap row pointers,
+  // as the initial factorization relies on the data being all in one chunk.
+  // variables that have findex >= 0 are *not* considered to be unbounded even
+  // if lo=-inf and hi=inf - this is because these limits may change during the
+  // solution process.
+
+  {
+    int *findex = m_findex;
+    btScalar *lo = m_lo, *hi = m_hi;
+    const int n = m_n;
+    for (int k = m_nub; k<n; ++k) {
+      if (findex && findex[k] >= 0) continue;
+      if (lo[k]==-BT_INFINITY && hi[k]==BT_INFINITY) {
+        btSwapProblem (m_A,m_x,m_b,m_w,lo,hi,m_p,m_state,findex,n,m_nub,k,m_nskip,0);
+        m_nub++;
+      }
+    }
+  }
+
+  // if there are unbounded variables at the start, factorize A up to that
+  // point and solve for x. this puts all indexes 0..nub-1 into C.
+  if (m_nub > 0) {
+    const int nub = m_nub;
+    {
+      btScalar *Lrow = m_L;
+      const int nskip = m_nskip;
+      for (int j=0; j<nub; Lrow+=nskip, ++j) memcpy(Lrow,BTAROW(j),(j+1)*sizeof(btScalar));
+    }
+    btFactorLDLT (m_L,m_d,nub,m_nskip);
+    memcpy (m_x,m_b,nub*sizeof(btScalar));
+    btSolveLDLT (m_L,m_d,m_x,nub,m_nskip);
+    btSetZero (m_w,nub);
+    {
+      int *C = m_C;
+      for (int k=0; k<nub; ++k) C[k] = k;
+    }
+    m_nC = nub;
+  }
+
+  // permute the indexes > nub such that all findex variables are at the end
+  if (m_findex) {
+    const int nub = m_nub;
+    int *findex = m_findex;
+    int num_at_end = 0;
+    for (int k=m_n-1; k >= nub; k--) {
+      if (findex[k] >= 0) {
+        btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,findex,m_n,k,m_n-1-num_at_end,m_nskip,1);
+        num_at_end++;
+      }
+    }
+  }
+
+  // print info about indexes
+  /*
+  {
+    const int n = m_n;
+    const int nub = m_nub;
+    for (int k=0; k<n; k++) {
+      if (k<nub) printf ("C");
+      else if (m_lo[k]==-BT_INFINITY && m_hi[k]==BT_INFINITY) printf ("c");
+      else printf (".");
+    }
+    printf ("\n");
+  }
+  */
+}
+
+
+void btLCP::transfer_i_to_C (int i)
+{
+  {
+    if (m_nC > 0) {
+      // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
+      {
+        const int nC = m_nC;
+        btScalar *const Ltgt = m_L + nC*m_nskip, *ell = m_ell;
+        for (int j=0; j<nC; ++j) Ltgt[j] = ell[j];
+      }
+      const int nC = m_nC;
+      m_d[nC] = btRecip (BTAROW(i)[i] - btLargeDot(m_ell,m_Dell,nC));
+    }
+    else {
+      m_d[0] = btRecip (BTAROW(i)[i]);
+    }
+
+    btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1);
+
+    const int nC = m_nC;
+    m_C[nC] = nC;
+    m_nC = nC + 1; // nC value is outdated after this line
+  }
+
+}
+
+
+void btLCP::transfer_i_from_N_to_C (int i)
+{
+  {
+    if (m_nC > 0) {
+      {
+        btScalar *const aptr = BTAROW(i);
+        btScalar *Dell = m_Dell;
+        const int *C = m_C;
+#   ifdef BTNUB_OPTIMIZATIONS
+        // if nub>0, initial part of aptr unpermuted
+        const int nub = m_nub;
+        int j=0;
+        for ( ; j<nub; ++j) Dell[j] = aptr[j];
+        const int nC = m_nC;
+        for ( ; j<nC; ++j) Dell[j] = aptr[C[j]];
+#   else
+        const int nC = m_nC;
+        for (int j=0; j<nC; ++j) Dell[j] = aptr[C[j]];
+#   endif
+      }
+      btSolveL1 (m_L,m_Dell,m_nC,m_nskip);
+      {
+        const int nC = m_nC;
+        btScalar *const Ltgt = m_L + nC*m_nskip;
+        btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d;
+        for (int j=0; j<nC; ++j) Ltgt[j] = ell[j] = Dell[j] * d[j];
+      }
+      const int nC = m_nC;
+      m_d[nC] = btRecip (BTAROW(i)[i] - btLargeDot(m_ell,m_Dell,nC));
+    }
+    else {
+      m_d[0] = btRecip (BTAROW(i)[i]);
+    }
+
+    btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1);
+
+    const int nC = m_nC;
+    m_C[nC] = nC;
+    m_nN--;
+    m_nC = nC + 1; // nC value is outdated after this line
+  }
+
+  // @@@ TO DO LATER
+  // if we just finish here then we'll go back and re-solve for
+  // delta_x. but actually we can be more efficient and incrementally
+  // update delta_x here. but if we do this, we wont have ell and Dell
+  // to use in updating the factorization later.
+
+}
+
+void btRemoveRowCol (btScalar *A, int n, int nskip, int r)
+{
+  btAssert(A && n > 0 && nskip >= n && r >= 0 && r < n);
+  if (r >= n-1) return;
+  if (r > 0) {
+    {
+      const size_t move_size = (n-r-1)*sizeof(btScalar);
+      btScalar *Adst = A + r;
+      for (int i=0; i<r; Adst+=nskip,++i) {
+        btScalar *Asrc = Adst + 1;
+        memmove (Adst,Asrc,move_size);
+      }
+    }
+    {
+      const size_t cpy_size = r*sizeof(btScalar);
+      btScalar *Adst = A + r * nskip;
+      for (int i=r; i<(n-1); ++i) {
+        btScalar *Asrc = Adst + nskip;
+        memcpy (Adst,Asrc,cpy_size);
+        Adst = Asrc;
+      }
+    }
+  }
+  {
+    const size_t cpy_size = (n-r-1)*sizeof(btScalar);
+    btScalar *Adst = A + r * (nskip + 1);
+    for (int i=r; i<(n-1); ++i) {
+      btScalar *Asrc = Adst + (nskip + 1);
+      memcpy (Adst,Asrc,cpy_size);
+      Adst = Asrc - 1;
+    }
+  }
+}
+
+
+
+
+void btLDLTAddTL (btScalar *L, btScalar *d, const btScalar *a, int n, int nskip, btAlignedObjectArray<btScalar>& scratch)
+{
+  btAssert (L && d && a && n > 0 && nskip >= n);
+
+  if (n < 2) return;
+  scratch.resize(2*nskip);
+  btScalar *W1 = &scratch[0];
+  
+  btScalar *W2 = W1 + nskip;
+
+  W1[0] = btScalar(0.0);
+  W2[0] = btScalar(0.0);
+  for (int j=1; j<n; ++j) {
+    W1[j] = W2[j] = (btScalar) (a[j] * SIMDSQRT12);
+  }
+  btScalar W11 = (btScalar) ((btScalar(0.5)*a[0]+1)*SIMDSQRT12);
+  btScalar W21 = (btScalar) ((btScalar(0.5)*a[0]-1)*SIMDSQRT12);
+
+  btScalar alpha1 = btScalar(1.0);
+  btScalar alpha2 = btScalar(1.0);
+
+  {
+    btScalar dee = d[0];
+    btScalar alphanew = alpha1 + (W11*W11)*dee;
+    btAssert(alphanew != btScalar(0.0));
+    dee /= alphanew;
+    btScalar gamma1 = W11 * dee;
+    dee *= alpha1;
+    alpha1 = alphanew;
+    alphanew = alpha2 - (W21*W21)*dee;
+    dee /= alphanew;
+    //btScalar gamma2 = W21 * dee;
+    alpha2 = alphanew;
+    btScalar k1 = btScalar(1.0) - W21*gamma1;
+    btScalar k2 = W21*gamma1*W11 - W21;
+    btScalar *ll = L + nskip;
+    for (int p=1; p<n; ll+=nskip, ++p) {
+      btScalar Wp = W1[p];
+      btScalar ell = *ll;
+      W1[p] =    Wp - W11*ell;
+      W2[p] = k1*Wp +  k2*ell;
+    }
+  }
+
+  btScalar *ll = L + (nskip + 1);
+  for (int j=1; j<n; ll+=nskip+1, ++j) {
+    btScalar k1 = W1[j];
+    btScalar k2 = W2[j];
+
+    btScalar dee = d[j];
+    btScalar alphanew = alpha1 + (k1*k1)*dee;
+    btAssert(alphanew != btScalar(0.0));
+    dee /= alphanew;
+    btScalar gamma1 = k1 * dee;
+    dee *= alpha1;
+    alpha1 = alphanew;
+    alphanew = alpha2 - (k2*k2)*dee;
+    dee /= alphanew;
+    btScalar gamma2 = k2 * dee;
+    dee *= alpha2;
+    d[j] = dee;
+    alpha2 = alphanew;
+
+    btScalar *l = ll + nskip;
+    for (int p=j+1; p<n; l+=nskip, ++p) {
+      btScalar ell = *l;
+      btScalar Wp = W1[p] - k1 * ell;
+      ell += gamma1 * Wp;
+      W1[p] = Wp;
+      Wp = W2[p] - k2 * ell;
+      ell -= gamma2 * Wp;
+      W2[p] = Wp;
+      *l = ell;
+    }
+  }
+}
+
+
+#define _BTGETA(i,j) (A[i][j])
+//#define _GETA(i,j) (A[(i)*nskip+(j)])
+#define BTGETA(i,j) ((i > j) ? _BTGETA(i,j) : _BTGETA(j,i))
+
+inline size_t btEstimateLDLTAddTLTmpbufSize(int nskip)
+{
+  return nskip * 2 * sizeof(btScalar);
+}
+
+
+void btLDLTRemove (btScalar **A, const int *p, btScalar *L, btScalar *d,
+    int n1, int n2, int r, int nskip, btAlignedObjectArray<btScalar>& scratch)
+{
+  btAssert(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 &&
+	   n1 >= n2 && nskip >= n1);
+  #ifdef BT_DEBUG
+	for (int i=0; i<n2; ++i) 
+		btAssert(p[i] >= 0 && p[i] < n1);
+  #endif
+
+  if (r==n2-1) {
+    return;		// deleting last row/col is easy
+  }
+  else {
+    size_t LDLTAddTL_size = btEstimateLDLTAddTLTmpbufSize(nskip);
+    btAssert(LDLTAddTL_size % sizeof(btScalar) == 0);
+	scratch.resize(nskip * 2+n2);
+    btScalar *tmp = &scratch[0];
+    if (r==0) {
+      btScalar *a = (btScalar *)((char *)tmp + LDLTAddTL_size);
+      const int p_0 = p[0];
+      for (int i=0; i<n2; ++i) {
+        a[i] = -BTGETA(p[i],p_0);
+      }
+      a[0] += btScalar(1.0);
+      btLDLTAddTL (L,d,a,n2,nskip,scratch);
+    }
+    else {
+      btScalar *t = (btScalar *)((char *)tmp + LDLTAddTL_size);
+      {
+        btScalar *Lcurr = L + r*nskip;
+        for (int i=0; i<r; ++Lcurr, ++i) {
+          btAssert(d[i] != btScalar(0.0));
+          t[i] = *Lcurr / d[i];
+        }
+      }
+      btScalar *a = t + r;
+      {
+        btScalar *Lcurr = L + r*nskip;
+        const int *pp_r = p + r, p_r = *pp_r;
+        const int n2_minus_r = n2-r;
+        for (int i=0; i<n2_minus_r; Lcurr+=nskip,++i) {
+          a[i] = btLargeDot(Lcurr,t,r) - BTGETA(pp_r[i],p_r);
+        }
+      }
+      a[0] += btScalar(1.0);
+      btLDLTAddTL (L + r*nskip+r, d+r, a, n2-r, nskip, scratch);
+    }
+  }
+
+  // snip out row/column r from L and d
+  btRemoveRowCol (L,n2,nskip,r);
+  if (r < (n2-1)) memmove (d+r,d+r+1,(n2-r-1)*sizeof(btScalar));
+}
+
+
+void btLCP::transfer_i_from_C_to_N (int i, btAlignedObjectArray<btScalar>& scratch)
+{
+  {
+    int *C = m_C;
+    // remove a row/column from the factorization, and adjust the
+    // indexes (black magic!)
+    int last_idx = -1;
+    const int nC = m_nC;
+    int j = 0;
+    for ( ; j<nC; ++j) {
+      if (C[j]==nC-1) {
+        last_idx = j;
+      }
+      if (C[j]==i) {
+        btLDLTRemove (m_A,C,m_L,m_d,m_n,nC,j,m_nskip,scratch);
+        int k;
+        if (last_idx == -1) {
+          for (k=j+1 ; k<nC; ++k) {
+            if (C[k]==nC-1) {
+              break;
+            }
+          }
+          btAssert (k < nC);
+        }
+        else {
+          k = last_idx;
+        }
+        C[k] = C[j];
+        if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*sizeof(int));
+        break;
+      }
+    }
+    btAssert (j < nC);
+
+    btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,i,nC-1,m_nskip,1);
+
+    m_nN++;
+    m_nC = nC - 1; // nC value is outdated after this line
+  }
+
+}
+
+
+void btLCP::pN_equals_ANC_times_qC (btScalar *p, btScalar *q)
+{
+  // we could try to make this matrix-vector multiplication faster using
+  // outer product matrix tricks, e.g. with the dMultidotX() functions.
+  // but i tried it and it actually made things slower on random 100x100
+  // problems because of the overhead involved. so we'll stick with the
+  // simple method for now.
+  const int nC = m_nC;
+  btScalar *ptgt = p + nC;
+  const int nN = m_nN;
+  for (int i=0; i<nN; ++i) {
+    ptgt[i] = btLargeDot (BTAROW(i+nC),q,nC);
+  }
+}
+
+
+void btLCP::pN_plusequals_ANi (btScalar *p, int i, int sign)
+{
+  const int nC = m_nC;
+  btScalar *aptr = BTAROW(i) + nC;
+  btScalar *ptgt = p + nC;
+  if (sign > 0) {
+    const int nN = m_nN;
+    for (int j=0; j<nN; ++j) ptgt[j] += aptr[j];
+  }
+  else {
+    const int nN = m_nN;
+    for (int j=0; j<nN; ++j) ptgt[j] -= aptr[j];
+  }
+}
+
+void btLCP::pC_plusequals_s_times_qC (btScalar *p, btScalar s, btScalar *q)
+{
+  const int nC = m_nC;
+  for (int i=0; i<nC; ++i) {
+    p[i] += s*q[i];
+  }
+}
+
+void btLCP::pN_plusequals_s_times_qN (btScalar *p, btScalar s, btScalar *q)
+{
+  const int nC = m_nC;
+  btScalar *ptgt = p + nC, *qsrc = q + nC;
+  const int nN = m_nN;
+  for (int i=0; i<nN; ++i) {
+    ptgt[i] += s*qsrc[i];
+  }
+}
+
+void btLCP::solve1 (btScalar *a, int i, int dir, int only_transfer)
+{
+  // the `Dell' and `ell' that are computed here are saved. if index i is
+  // later added to the factorization then they can be reused.
+  //
+  // @@@ question: do we need to solve for entire delta_x??? yes, but
+  //     only if an x goes below 0 during the step.
+
+  if (m_nC > 0) {
+    {
+      btScalar *Dell = m_Dell;
+      int *C = m_C;
+      btScalar *aptr = BTAROW(i);
+#   ifdef BTNUB_OPTIMIZATIONS
+      // if nub>0, initial part of aptr[] is guaranteed unpermuted
+      const int nub = m_nub;
+      int j=0;
+      for ( ; j<nub; ++j) Dell[j] = aptr[j];
+      const int nC = m_nC;
+      for ( ; j<nC; ++j) Dell[j] = aptr[C[j]];
+#   else
+      const int nC = m_nC;
+      for (int j=0; j<nC; ++j) Dell[j] = aptr[C[j]];
+#   endif
+    }
+    btSolveL1 (m_L,m_Dell,m_nC,m_nskip);
+    {
+      btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d;
+      const int nC = m_nC;
+      for (int j=0; j<nC; ++j) ell[j] = Dell[j] * d[j];
+    }
+
+    if (!only_transfer) {
+      btScalar *tmp = m_tmp, *ell = m_ell;
+      {
+        const int nC = m_nC;
+        for (int j=0; j<nC; ++j) tmp[j] = ell[j];
+      }
+      btSolveL1T (m_L,tmp,m_nC,m_nskip);
+      if (dir > 0) {
+        int *C = m_C;
+        btScalar *tmp = m_tmp;
+        const int nC = m_nC;
+        for (int j=0; j<nC; ++j) a[C[j]] = -tmp[j];
+      } else {
+        int *C = m_C;
+        btScalar *tmp = m_tmp;
+        const int nC = m_nC;
+        for (int j=0; j<nC; ++j) a[C[j]] = tmp[j];
+      }
+    }
+  }
+}
+
+
+void btLCP::unpermute()
+{
+  // now we have to un-permute x and w
+  {
+    memcpy (m_tmp,m_x,m_n*sizeof(btScalar));
+    btScalar *x = m_x, *tmp = m_tmp;
+    const int *p = m_p;
+    const int n = m_n;
+    for (int j=0; j<n; ++j) x[p[j]] = tmp[j];
+  }
+  {
+    memcpy (m_tmp,m_w,m_n*sizeof(btScalar));
+    btScalar *w = m_w, *tmp = m_tmp;
+    const int *p = m_p;
+    const int n = m_n;
+    for (int j=0; j<n; ++j) w[p[j]] = tmp[j];
+  }
+}
+
+#endif // btLCP_FAST
+
+
+//***************************************************************************
+// an optimized Dantzig LCP driver routine for the lo-hi LCP problem.
+
+bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b,
+                btScalar* outer_w, int nub, btScalar *lo, btScalar *hi, int *findex, btDantzigScratchMemory& scratchMem)
+{
+	s_error = false;
+
+//	printf("btSolveDantzigLCP n=%d\n",n);
+  btAssert (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n);
+  btAssert(outer_w);
+
+#ifdef BT_DEBUG
+  {
+    // check restrictions on lo and hi
+    for (int k=0; k<n; ++k) 
+		btAssert (lo[k] <= 0 && hi[k] >= 0);
+  }
+# endif
+
+
+  // if all the variables are unbounded then we can just factor, solve,
+  // and return
+  if (nub >= n) 
+  {
+   
+
+    int nskip = (n);
+    btFactorLDLT (A, outer_w, n, nskip);
+    btSolveLDLT (A, outer_w, b, n, nskip);
+    memcpy (x, b, n*sizeof(btScalar));
+
+    return !s_error;
+  }
+
+  const int nskip = (n);
+  scratchMem.L.resize(n*nskip);
+
+  scratchMem.d.resize(n);
+
+  btScalar *w = outer_w;
+  scratchMem.delta_w.resize(n);
+  scratchMem.delta_x.resize(n);
+  scratchMem.Dell.resize(n);
+  scratchMem.ell.resize(n);
+  scratchMem.Arows.resize(n);
+  scratchMem.p.resize(n);
+  scratchMem.C.resize(n);
+
+  // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
+  scratchMem.state.resize(n);
+
+
+  // create LCP object. note that tmp is set to delta_w to save space, this
+  // optimization relies on knowledge of how tmp is used, so be careful!
+  btLCP lcp(n,nskip,nub,A,x,b,w,lo,hi,&scratchMem.L[0],&scratchMem.d[0],&scratchMem.Dell[0],&scratchMem.ell[0],&scratchMem.delta_w[0],&scratchMem.state[0],findex,&scratchMem.p[0],&scratchMem.C[0],&scratchMem.Arows[0]);
+  int adj_nub = lcp.getNub();
+
+  // loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the
+  // LCP conditions then i is added to the appropriate index set. otherwise
+  // x(i),w(i) is driven either +ve or -ve to force it to the valid region.
+  // as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
+  // while driving x(i) we maintain the LCP conditions on the other variables
+  // 0..i-1. we do this by watching out for other x(i),w(i) values going
+  // outside the valid region, and then switching them between index sets
+  // when that happens.
+
+  bool hit_first_friction_index = false;
+  for (int i=adj_nub; i<n; ++i) 
+  {
+    s_error = false;
+    // the index i is the driving index and indexes i+1..n-1 are "dont care",
+    // i.e. when we make changes to the system those x's will be zero and we
+    // don't care what happens to those w's. in other words, we only consider
+    // an (i+1)*(i+1) sub-problem of A*x=b+w.
+
+    // if we've hit the first friction index, we have to compute the lo and
+    // hi values based on the values of x already computed. we have been
+    // permuting the indexes, so the values stored in the findex vector are
+    // no longer valid. thus we have to temporarily unpermute the x vector. 
+    // for the purposes of this computation, 0*infinity = 0 ... so if the
+    // contact constraint's normal force is 0, there should be no tangential
+    // force applied.
+
+    if (!hit_first_friction_index && findex && findex[i] >= 0) {
+      // un-permute x into delta_w, which is not being used at the moment
+      for (int j=0; j<n; ++j) scratchMem.delta_w[scratchMem.p[j]] = x[j];
+
+      // set lo and hi values
+      for (int k=i; k<n; ++k) {
+        btScalar wfk = scratchMem.delta_w[findex[k]];
+        if (wfk == 0) {
+          hi[k] = 0;
+          lo[k] = 0;
+        }
+        else {
+          hi[k] = btFabs (hi[k] * wfk);
+          lo[k] = -hi[k];
+        }
+      }
+      hit_first_friction_index = true;
+    }
+
+    // thus far we have not even been computing the w values for indexes
+    // greater than i, so compute w[i] now.
+    w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i];
+
+    // if lo=hi=0 (which can happen for tangential friction when normals are
+    // 0) then the index will be assigned to set N with some state. however,
+    // set C's line has zero size, so the index will always remain in set N.
+    // with the "normal" switching logic, if w changed sign then the index
+    // would have to switch to set C and then back to set N with an inverted
+    // state. this is pointless, and also computationally expensive. to
+    // prevent this from happening, we use the rule that indexes with lo=hi=0
+    // will never be checked for set changes. this means that the state for
+    // these indexes may be incorrect, but that doesn't matter.
+
+    // see if x(i),w(i) is in a valid region
+    if (lo[i]==0 && w[i] >= 0) {
+      lcp.transfer_i_to_N (i);
+      scratchMem.state[i] = false;
+    }
+    else if (hi[i]==0 && w[i] <= 0) {
+      lcp.transfer_i_to_N (i);
+      scratchMem.state[i] = true;
+    }
+    else if (w[i]==0) {
+      // this is a degenerate case. by the time we get to this test we know
+      // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
+      // and similarly that hi > 0. this means that the line segment
+      // corresponding to set C is at least finite in extent, and we are on it.
+      // NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C()
+      lcp.solve1 (&scratchMem.delta_x[0],i,0,1);
+
+      lcp.transfer_i_to_C (i);
+    }
+    else {
+      // we must push x(i) and w(i)
+      for (;;) {
+        int dir;
+        btScalar dirf;
+        // find direction to push on x(i)
+        if (w[i] <= 0) {
+          dir = 1;
+          dirf = btScalar(1.0);
+        }
+        else {
+          dir = -1;
+          dirf = btScalar(-1.0);
+        }
+
+        // compute: delta_x(C) = -dir*A(C,C)\A(C,i)
+        lcp.solve1 (&scratchMem.delta_x[0],i,dir);
+
+        // note that delta_x[i] = dirf, but we wont bother to set it
+
+        // compute: delta_w = A*delta_x ... note we only care about
+        // delta_w(N) and delta_w(i), the rest is ignored
+        lcp.pN_equals_ANC_times_qC (&scratchMem.delta_w[0],&scratchMem.delta_x[0]);
+        lcp.pN_plusequals_ANi (&scratchMem.delta_w[0],i,dir);
+        scratchMem.delta_w[i] = lcp.AiC_times_qC (i,&scratchMem.delta_x[0]) + lcp.Aii(i)*dirf;
+
+        // find largest step we can take (size=s), either to drive x(i),w(i)
+        // to the valid LCP region or to drive an already-valid variable
+        // outside the valid region.
+
+        int cmd = 1;		// index switching command
+        int si = 0;		// si = index to switch if cmd>3
+        btScalar s = -w[i]/scratchMem.delta_w[i];
+        if (dir > 0) {
+          if (hi[i] < BT_INFINITY) {
+            btScalar s2 = (hi[i]-x[i])*dirf;	// was (hi[i]-x[i])/dirf	// step to x(i)=hi(i)
+            if (s2 < s) {
+              s = s2;
+              cmd = 3;
+            }
+          }
+        }
+        else {
+          if (lo[i] > -BT_INFINITY) {
+            btScalar s2 = (lo[i]-x[i])*dirf;	// was (lo[i]-x[i])/dirf	// step to x(i)=lo(i)
+            if (s2 < s) {
+              s = s2;
+              cmd = 2;
+            }
+          }
+        }
+
+        {
+          const int numN = lcp.numN();
+          for (int k=0; k < numN; ++k) {
+            const int indexN_k = lcp.indexN(k);
+            if (!scratchMem.state[indexN_k] ? scratchMem.delta_w[indexN_k] < 0 : scratchMem.delta_w[indexN_k] > 0) {
+                // don't bother checking if lo=hi=0
+                if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue;
+                btScalar s2 = -w[indexN_k] / scratchMem.delta_w[indexN_k];
+                if (s2 < s) {
+                  s = s2;
+                  cmd = 4;
+                  si = indexN_k;
+                }
+            }
+          }
+        }
+
+        {
+          const int numC = lcp.numC();
+          for (int k=adj_nub; k < numC; ++k) {
+            const int indexC_k = lcp.indexC(k);
+            if (scratchMem.delta_x[indexC_k] < 0 && lo[indexC_k] > -BT_INFINITY) {
+              btScalar s2 = (lo[indexC_k]-x[indexC_k]) / scratchMem.delta_x[indexC_k];
+              if (s2 < s) {
+                s = s2;
+                cmd = 5;
+                si = indexC_k;
+              }
+            }
+            if (scratchMem.delta_x[indexC_k] > 0 && hi[indexC_k] < BT_INFINITY) {
+              btScalar s2 = (hi[indexC_k]-x[indexC_k]) / scratchMem.delta_x[indexC_k];
+              if (s2 < s) {
+                s = s2;
+                cmd = 6;
+                si = indexC_k;
+              }
+            }
+          }
+        }
+
+        //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
+        //			     "C->NL","C->NH"};
+        //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
+
+        // if s <= 0 then we've got a problem. if we just keep going then
+        // we're going to get stuck in an infinite loop. instead, just cross
+        // our fingers and exit with the current solution.
+        if (s <= btScalar(0.0)) 
+		{
+//          printf("LCP internal error, s <= 0 (s=%.4e)",(double)s);
+          if (i < n) {
+            btSetZero (x+i,n-i);
+            btSetZero (w+i,n-i);
+          }
+          s_error = true;
+          break;
+        }
+
+        // apply x = x + s * delta_x
+        lcp.pC_plusequals_s_times_qC (x, s, &scratchMem.delta_x[0]);
+        x[i] += s * dirf;
+
+        // apply w = w + s * delta_w
+        lcp.pN_plusequals_s_times_qN (w, s, &scratchMem.delta_w[0]);
+        w[i] += s * scratchMem.delta_w[i];
+
+//        void *tmpbuf;
+        // switch indexes between sets if necessary
+        switch (cmd) {
+        case 1:		// done
+          w[i] = 0;
+          lcp.transfer_i_to_C (i);
+          break;
+        case 2:		// done
+          x[i] = lo[i];
+          scratchMem.state[i] = false;
+          lcp.transfer_i_to_N (i);
+          break;
+        case 3:		// done
+          x[i] = hi[i];
+          scratchMem.state[i] = true;
+          lcp.transfer_i_to_N (i);
+          break;
+        case 4:		// keep going
+          w[si] = 0;
+          lcp.transfer_i_from_N_to_C (si);
+          break;
+        case 5:		// keep going
+          x[si] = lo[si];
+          scratchMem.state[si] = false;
+		  lcp.transfer_i_from_C_to_N (si, scratchMem.m_scratch);
+          break;
+        case 6:		// keep going
+          x[si] = hi[si];
+          scratchMem.state[si] = true;
+          lcp.transfer_i_from_C_to_N (si, scratchMem.m_scratch);
+          break;
+        }
+
+        if (cmd <= 3) break;
+      } // for (;;)
+    } // else
+
+    if (s_error) 
+	{
+      break;
+    }
+  } // for (int i=adj_nub; i<n; ++i)
+
+  lcp.unpermute();
+
+
+  return !s_error;
+}
+

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h

@@ -0,0 +1,77 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+ * All rights reserved.  Email: [email protected]   Web: www.q12.org          *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of                                          * 
+ *   The BSD-style license that is included with this library in         *
+ *   the file LICENSE-BSD.TXT.                                           *
+ *                                                                       *
+ * This library is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i)
+satisfies one of
+	(1) x = lo, w >= 0
+	(2) x = hi, w <= 0
+	(3) lo < x < hi, w = 0
+A is a matrix of dimension n*n, everything else is a vector of size n*1.
+lo and hi can be +/- dInfinity as needed. the first `nub' variables are
+unbounded, i.e. hi and lo are assumed to be +/- dInfinity.
+
+we restrict lo(i) <= 0 and hi(i) >= 0.
+
+the original data (A,b) may be modified by this function.
+
+if the `findex' (friction index) parameter is nonzero, it points to an array
+of index values. in this case constraints that have findex[i] >= 0 are
+special. all non-special constraints are solved for, then the lo and hi values
+for the special constraints are set:
+  hi[i] = abs( hi[i] * x[findex[i]] )
+  lo[i] = -hi[i]
+and the solution continues. this mechanism allows a friction approximation
+to be implemented. the first `nub' variables are assumed to have findex < 0.
+
+*/
+
+
+#ifndef _BT_LCP_H_
+#define _BT_LCP_H_
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <assert.h>
+
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+struct btDantzigScratchMemory
+{
+	btAlignedObjectArray<btScalar> m_scratch;
+	btAlignedObjectArray<btScalar> L;
+	btAlignedObjectArray<btScalar> d;
+	btAlignedObjectArray<btScalar> delta_w;
+	btAlignedObjectArray<btScalar> delta_x;
+	btAlignedObjectArray<btScalar> Dell;
+	btAlignedObjectArray<btScalar> ell;
+	btAlignedObjectArray<btScalar*> Arows;
+	btAlignedObjectArray<int> p;
+	btAlignedObjectArray<int> C;
+	btAlignedObjectArray<bool> state;
+};
+
+//return false if solving failed
+bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w,
+	int nub, btScalar *lo, btScalar *hi, int *findex,btDantzigScratchMemory& scratch);
+
+
+
+#endif //_BT_LCP_H_

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h

@@ -0,0 +1,112 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-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.
+*/
+///original version written by Erwin Coumans, October 2013
+
+#ifndef BT_DANTZIG_SOLVER_H
+#define BT_DANTZIG_SOLVER_H
+
+#include "btMLCPSolverInterface.h"
+#include "btDantzigLCP.h"
+
+
+class btDantzigSolver : public btMLCPSolverInterface
+{
+protected:
+
+	btScalar m_acceptableUpperLimitSolution;
+
+	btAlignedObjectArray<char>	m_tempBuffer;
+
+	btAlignedObjectArray<btScalar> m_A;
+	btAlignedObjectArray<btScalar> m_b;
+	btAlignedObjectArray<btScalar> m_x;
+	btAlignedObjectArray<btScalar> m_lo;
+	btAlignedObjectArray<btScalar> m_hi;
+	btAlignedObjectArray<int>	m_dependencies;
+	btDantzigScratchMemory m_scratchMemory;
+public:
+
+	btDantzigSolver()
+		:m_acceptableUpperLimitSolution(btScalar(1000))
+	{
+	}
+
+	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	{
+		bool result = true;
+		int n = b.rows();
+		if (n)
+		{
+			int nub = 0;
+			btAlignedObjectArray<btScalar> ww;
+			ww.resize(n);
+	
+
+			const btScalar* Aptr = A.getBufferPointer();
+			m_A.resize(n*n);
+			for (int i=0;i<n*n;i++)
+			{
+				m_A[i] = Aptr[i];
+
+			}
+
+			m_b.resize(n);
+			m_x.resize(n);
+			m_lo.resize(n);
+			m_hi.resize(n);
+			m_dependencies.resize(n);
+			for (int i=0;i<n;i++)
+			{
+				m_lo[i] = lo[i];
+				m_hi[i] = hi[i];
+				m_b[i] = b[i];
+				m_x[i] = x[i];
+				m_dependencies[i] = limitDependency[i];
+			}
+
+
+			result = btSolveDantzigLCP (n,&m_A[0],&m_x[0],&m_b[0],&ww[0],nub,&m_lo[0],&m_hi[0],&m_dependencies[0],m_scratchMemory);
+			if (!result)
+				return result;
+
+//			printf("numAllocas = %d\n",numAllocas);
+			for (int i=0;i<n;i++)
+			{
+				volatile btScalar xx = m_x[i];
+				if (xx != m_x[i])
+					return false;
+				if (x[i] >= m_acceptableUpperLimitSolution)
+				{
+					return false;
+				}
+
+				if (x[i] <= -m_acceptableUpperLimitSolution)
+				{
+					return false;
+				}
+			}
+
+			for (int i=0;i<n;i++)
+			{
+				x[i] = m_x[i];
+			}
+			
+		}
+
+		return result;
+	}
+};
+
+#endif //BT_DANTZIG_SOLVER_H

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp

@@ -0,0 +1,626 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-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.
+*/
+///original version written by Erwin Coumans, October 2013
+
+#include "btMLCPSolver.h"
+#include "LinearMath/btMatrixX.h"
+#include "LinearMath/btQuickprof.h"
+#include "btSolveProjectedGaussSeidel.h"
+
+btMLCPSolver::btMLCPSolver(	 btMLCPSolverInterface* solver)
+:m_solver(solver),
+m_fallback(0)
+{
+}
+
+btMLCPSolver::~btMLCPSolver()
+{
+}
+
+bool gUseMatrixMultiply = false;
+bool interleaveContactAndFriction = false;
+
+btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies, numBodiesUnUsed, manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+
+	{
+		BT_PROFILE("gather constraint data");
+
+		int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2;
+
+
+		int numBodies = m_tmpSolverBodyPool.size();
+		m_allConstraintArray.resize(0);
+		m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
+		btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
+	//	printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size());
+
+		int dindex = 0;
+		for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++)
+		{
+			m_allConstraintArray.push_back(m_tmpSolverNonContactConstraintPool[i]);
+			m_limitDependencies[dindex++] = -1;
+		}
+ 
+		///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
+		
+		int firstContactConstraintOffset=dindex;
+
+		if (interleaveContactAndFriction)
+		{
+			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
+			{
+				m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+				m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
+				int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact));
+				m_limitDependencies[dindex++] = findex +firstContactConstraintOffset;
+				if (numFrictionPerContact==2)
+				{
+					m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
+					m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
+				}
+			}
+		} else
+		{
+			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
+			{
+				m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+			}
+			for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++)
+			{
+				m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i]);
+				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
+			}
+			
+		}
+
+
+		if (!m_allConstraintArray.size())
+		{
+			m_A.resize(0,0);
+			m_b.resize(0);
+			m_x.resize(0);
+			m_lo.resize(0);
+			m_hi.resize(0);
+			return 0.f;
+		}
+	}
+
+	
+	if (gUseMatrixMultiply)
+	{
+		BT_PROFILE("createMLCP");
+		createMLCP(infoGlobal);
+	}
+	else
+	{
+		BT_PROFILE("createMLCPFast");
+		createMLCPFast(infoGlobal);
+	}
+
+	return 0.f;
+}
+
+bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
+{
+	bool result = true;
+
+	if (m_A.rows()==0)
+		return true;
+
+	//if using split impulse, we solve 2 separate (M)LCPs
+	if (infoGlobal.m_splitImpulse)
+	{
+		btMatrixXu Acopy = m_A;
+		btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
+//		printf("solve first LCP\n");
+		result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
+		if (result)
+			result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations );
+
+	} else
+	{
+		result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
+	}
+	return result;
+}
+
+struct btJointNode
+{
+	int jointIndex;     // pointer to enclosing dxJoint object
+	int otherBodyIndex;       // *other* body this joint is connected to
+	int nextJointNodeIndex;//-1 for null
+	int constraintRowIndex;
+};
+
+
+
+void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
+{
+	int numContactRows = interleaveContactAndFriction ? 3 : 1;
+
+	int numConstraintRows = m_allConstraintArray.size();
+	int n = numConstraintRows;
+	{
+		BT_PROFILE("init b (rhs)");
+		m_b.resize(numConstraintRows);
+		m_bSplit.resize(numConstraintRows);
+		//m_b.setZero();
+		for (int i=0;i<numConstraintRows ;i++)
+		{
+			if (m_allConstraintArray[i].m_jacDiagABInv)
+			{
+				m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
+				m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
+			}
+
+		}
+	}
+
+	btScalar* w = 0;
+	int nub = 0;
+
+	m_lo.resize(numConstraintRows);
+	m_hi.resize(numConstraintRows);
+ 
+	{
+		BT_PROFILE("init lo/ho");
+
+		for (int i=0;i<numConstraintRows;i++)
+		{
+			if (0)//m_limitDependencies[i]>=0)
+			{
+				m_lo[i] = -BT_INFINITY;
+				m_hi[i] = BT_INFINITY;
+			} else
+			{
+				m_lo[i] = m_allConstraintArray[i].m_lowerLimit;
+				m_hi[i] = m_allConstraintArray[i].m_upperLimit;
+			}
+		}
+	}
+
+	//
+	int m=m_allConstraintArray.size();
+
+	int numBodies = m_tmpSolverBodyPool.size();
+	btAlignedObjectArray<int> bodyJointNodeArray;
+	{
+		BT_PROFILE("bodyJointNodeArray.resize");
+		bodyJointNodeArray.resize(numBodies,-1);
+	}
+	btAlignedObjectArray<btJointNode> jointNodeArray;
+	{
+		BT_PROFILE("jointNodeArray.reserve");
+		jointNodeArray.reserve(2*m_allConstraintArray.size());
+	}
+
+	static btMatrixXu J3;
+	{
+		BT_PROFILE("J3.resize");
+		J3.resize(2*m,8);
+	}
+	static btMatrixXu JinvM3;
+	{
+		BT_PROFILE("JinvM3.resize/setZero");
+
+		JinvM3.resize(2*m,8);
+		JinvM3.setZero();
+		J3.setZero();
+	}
+	int cur=0;
+	int rowOffset = 0;
+	static btAlignedObjectArray<int> ofs;
+	{
+		BT_PROFILE("ofs resize");
+		ofs.resize(0);
+		ofs.resizeNoInitialize(m_allConstraintArray.size());
+	}				
+	{
+		BT_PROFILE("Compute J and JinvM");
+		int c=0;
+
+		int numRows = 0;
+
+		for (int i=0;i<m_allConstraintArray.size();i+=numRows,c++)
+		{
+			ofs[c] = rowOffset;
+			int sbA = m_allConstraintArray[i].m_solverBodyIdA;
+			int sbB = m_allConstraintArray[i].m_solverBodyIdB;
+			btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+			btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+			numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
+			if (orgBodyA)
+			{
+				{
+					int slotA=-1;
+					//find free jointNode slot for sbA
+					slotA =jointNodeArray.size();
+					jointNodeArray.expand();//NonInitializing();
+					int prevSlot = bodyJointNodeArray[sbA];
+					bodyJointNodeArray[sbA] = slotA;
+					jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
+					jointNodeArray[slotA].jointIndex = c;
+					jointNodeArray[slotA].constraintRowIndex = i;
+					jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
+				}
+				for (int row=0;row<numRows;row++,cur++)
+				{
+					btVector3 normalInvMass =				m_allConstraintArray[i+row].m_contactNormal1 *		orgBodyA->getInvMass();
+					btVector3 relPosCrossNormalInvInertia = m_allConstraintArray[i+row].m_relpos1CrossNormal *	orgBodyA->getInvInertiaTensorWorld();
+
+					for (int r=0;r<3;r++)
+					{
+						J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal1[r]);
+						J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos1CrossNormal[r]);
+						JinvM3.setElem(cur,r,normalInvMass[r]);
+						JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]);
+					}
+					J3.setElem(cur,3,0);
+					JinvM3.setElem(cur,3,0);
+					J3.setElem(cur,7,0);
+					JinvM3.setElem(cur,7,0);
+				}
+			} else
+			{
+				cur += numRows;
+			}
+			if (orgBodyB)
+			{
+
+				{
+					int slotB=-1;
+					//find free jointNode slot for sbA
+					slotB =jointNodeArray.size();
+					jointNodeArray.expand();//NonInitializing();
+					int prevSlot = bodyJointNodeArray[sbB];
+					bodyJointNodeArray[sbB] = slotB;
+					jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
+					jointNodeArray[slotB].jointIndex = c;
+					jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
+					jointNodeArray[slotB].constraintRowIndex = i;
+				}
+
+				for (int row=0;row<numRows;row++,cur++)
+				{
+					btVector3 normalInvMassB = m_allConstraintArray[i+row].m_contactNormal2*orgBodyB->getInvMass();
+					btVector3 relPosInvInertiaB = m_allConstraintArray[i+row].m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
+
+					for (int r=0;r<3;r++)
+					{
+						J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal2[r]);
+						J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos2CrossNormal[r]);
+						JinvM3.setElem(cur,r,normalInvMassB[r]);
+						JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]);
+					}
+					J3.setElem(cur,3,0);
+					JinvM3.setElem(cur,3,0);
+					J3.setElem(cur,7,0);
+					JinvM3.setElem(cur,7,0);
+				}
+			}
+			else
+			{
+				cur += numRows;
+			}
+			rowOffset+=numRows;
+
+		}
+		
+	}
+
+
+	//compute JinvM = J*invM.
+	const btScalar* JinvM = JinvM3.getBufferPointer();
+
+	const btScalar* Jptr = J3.getBufferPointer();
+	{
+		BT_PROFILE("m_A.resize");
+		m_A.resize(n,n);
+	}
+	
+	{
+		BT_PROFILE("m_A.setZero");
+		m_A.setZero();
+	}
+	int c=0;
+	{
+		int numRows = 0;
+		BT_PROFILE("Compute A");
+		for (int i=0;i<m_allConstraintArray.size();i+= numRows,c++)
+		{
+			int row__ = ofs[c];
+			int sbA = m_allConstraintArray[i].m_solverBodyIdA;
+			int sbB = m_allConstraintArray[i].m_solverBodyIdB;
+			btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+			btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+			numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
+					
+			const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
+
+			{
+				int startJointNodeA = bodyJointNodeArray[sbA];
+				while (startJointNodeA>=0)
+				{
+					int j0 = jointNodeArray[startJointNodeA].jointIndex;
+					int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
+					if (j0<c)
+					{
+								 
+						int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
+						size_t ofsother = (m_allConstraintArray[cr0].m_solverBodyIdB == sbA) ? 8*numRowsOther  : 0;
+						//printf("%d joint i %d and j0: %d: ",count++,i,j0);
+						m_A.multiplyAdd2_p8r ( JinvMrow, 
+						Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther,  row__,ofs[j0]);
+					}
+					startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
+				}
+			}
+
+			{
+				int startJointNodeB = bodyJointNodeArray[sbB];
+				while (startJointNodeB>=0)
+				{
+					int j1 = jointNodeArray[startJointNodeB].jointIndex;
+					int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
+
+					if (j1<c)
+					{
+						int numRowsOther =  cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
+						size_t ofsother = (m_allConstraintArray[cj1].m_solverBodyIdB == sbB) ? 8*numRowsOther  : 0;
+						m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, 
+						Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]);
+					}
+					startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
+				}
+			}
+		}
+
+		{
+			BT_PROFILE("compute diagonal");
+			// compute diagonal blocks of m_A
+
+			int  row__ = 0;
+			int numJointRows = m_allConstraintArray.size();
+
+			int jj=0;
+			for (;row__<numJointRows;)
+			{
+
+				int sbA = m_allConstraintArray[row__].m_solverBodyIdA;
+				int sbB = m_allConstraintArray[row__].m_solverBodyIdB;
+				btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+				btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+
+				const unsigned int infom =  row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
+				
+				const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
+				const btScalar *Jrow = Jptr + 2*8*(size_t)row__;
+				m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__);
+				if (orgBodyB) 
+				{
+					m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom,  row__,row__);
+				}
+				row__ += infom;
+				jj++;
+			}
+		}
+	}
+
+	///todo: use proper cfm values from the constraints (getInfo2)
+	if (1)
+	{
+		// add cfm to the diagonal of m_A
+		for ( int i=0; i<m_A.rows(); ++i) 
+		{
+			float cfm = 0.00001f;
+			m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
+		}
+	}
+				   
+	///fill the upper triangle of the matrix, to make it symmetric
+	{
+		BT_PROFILE("fill the upper triangle ");
+		m_A.copyLowerToUpperTriangle();
+	}
+
+	{
+		BT_PROFILE("resize/init x");
+		m_x.resize(numConstraintRows);
+		m_xSplit.resize(numConstraintRows);
+
+		if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
+		{
+			for (int i=0;i<m_allConstraintArray.size();i++)
+			{
+				const btSolverConstraint& c = m_allConstraintArray[i];
+				m_x[i]=c.m_appliedImpulse;
+				m_xSplit[i] = c.m_appliedPushImpulse;
+			}
+		} else
+		{
+			m_x.setZero();
+			m_xSplit.setZero();
+		}
+	}
+
+}
+
+void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
+{
+	int numBodies = this->m_tmpSolverBodyPool.size();
+	int numConstraintRows = m_allConstraintArray.size();
+
+	m_b.resize(numConstraintRows);
+	if (infoGlobal.m_splitImpulse)
+		m_bSplit.resize(numConstraintRows);
+ 
+	for (int i=0;i<numConstraintRows ;i++)
+	{
+		if (m_allConstraintArray[i].m_jacDiagABInv)
+		{
+			m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
+			if (infoGlobal.m_splitImpulse)
+				m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
+		}
+	}
+ 
+	static btMatrixXu Minv;
+	Minv.resize(6*numBodies,6*numBodies);
+	Minv.setZero();
+	for (int i=0;i<numBodies;i++)
+	{
+		const btSolverBody& rb = m_tmpSolverBodyPool[i];
+		const btVector3& invMass = rb.m_invMass;
+		setElem(Minv,i*6+0,i*6+0,invMass[0]);
+		setElem(Minv,i*6+1,i*6+1,invMass[1]);
+		setElem(Minv,i*6+2,i*6+2,invMass[2]);
+		btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody;
+ 
+		for (int r=0;r<3;r++)
+			for (int c=0;c<3;c++)
+				setElem(Minv,i*6+3+r,i*6+3+c,orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0);
+	}
+ 
+	static btMatrixXu J;
+	J.resize(numConstraintRows,6*numBodies);
+	J.setZero();
+ 
+	m_lo.resize(numConstraintRows);
+	m_hi.resize(numConstraintRows);
+ 
+	for (int i=0;i<numConstraintRows;i++)
+	{
+
+		m_lo[i] = m_allConstraintArray[i].m_lowerLimit;
+		m_hi[i] = m_allConstraintArray[i].m_upperLimit;
+ 
+		int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA;
+		int bodyIndex1 = m_allConstraintArray[i].m_solverBodyIdB;
+		if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody)
+		{
+			setElem(J,i,6*bodyIndex0+0,m_allConstraintArray[i].m_contactNormal1[0]);
+			setElem(J,i,6*bodyIndex0+1,m_allConstraintArray[i].m_contactNormal1[1]);
+			setElem(J,i,6*bodyIndex0+2,m_allConstraintArray[i].m_contactNormal1[2]);
+			setElem(J,i,6*bodyIndex0+3,m_allConstraintArray[i].m_relpos1CrossNormal[0]);
+			setElem(J,i,6*bodyIndex0+4,m_allConstraintArray[i].m_relpos1CrossNormal[1]);
+			setElem(J,i,6*bodyIndex0+5,m_allConstraintArray[i].m_relpos1CrossNormal[2]);
+		}
+		if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody)
+		{
+			setElem(J,i,6*bodyIndex1+0,m_allConstraintArray[i].m_contactNormal2[0]);
+			setElem(J,i,6*bodyIndex1+1,m_allConstraintArray[i].m_contactNormal2[1]);
+			setElem(J,i,6*bodyIndex1+2,m_allConstraintArray[i].m_contactNormal2[2]);
+			setElem(J,i,6*bodyIndex1+3,m_allConstraintArray[i].m_relpos2CrossNormal[0]);
+			setElem(J,i,6*bodyIndex1+4,m_allConstraintArray[i].m_relpos2CrossNormal[1]);
+			setElem(J,i,6*bodyIndex1+5,m_allConstraintArray[i].m_relpos2CrossNormal[2]);
+		}
+	}
+ 
+	static btMatrixXu J_transpose;
+	J_transpose= J.transpose();
+
+	static btMatrixXu tmp;
+
+	{
+		{
+			BT_PROFILE("J*Minv");
+			tmp = J*Minv;
+
+		}
+		{
+			BT_PROFILE("J*tmp");
+			m_A = tmp*J_transpose;
+		}
+	}
+
+	if (1)
+	{
+		///todo: use proper cfm values from the constraints (getInfo2)
+		// add cfm to the diagonal of m_A
+		for ( int i=0; i<m_A.rows(); ++i) 
+		{
+			float cfm = 0.0001f;
+			m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
+		}
+	}
+
+	m_x.resize(numConstraintRows);
+	if (infoGlobal.m_splitImpulse)
+		m_xSplit.resize(numConstraintRows);
+//	m_x.setZero();
+
+	for (int i=0;i<m_allConstraintArray.size();i++)
+	{
+		const btSolverConstraint& c = m_allConstraintArray[i];
+		m_x[i]=c.m_appliedImpulse;
+		if (infoGlobal.m_splitImpulse)
+			m_xSplit[i] = c.m_appliedPushImpulse;
+	}
+
+}
+
+
+btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	bool result = true;
+	{
+		BT_PROFILE("solveMLCP");
+//		printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
+		result = solveMLCP(infoGlobal);
+	}
+
+	//check if solution is valid, and otherwise fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations
+	if (result)
+	{
+		BT_PROFILE("process MLCP results");
+		for (int i=0;i<m_allConstraintArray.size();i++)
+		{
+			{
+				btSolverConstraint& c = m_allConstraintArray[i];
+				int sbA = c.m_solverBodyIdA;
+				int sbB = c.m_solverBodyIdB;
+				btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+				btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+				btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
+				btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
+ 
+				solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_x[i]);
+				solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_x[i]);
+				if (infoGlobal.m_splitImpulse)
+				{
+					solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_xSplit[i]);
+					solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_xSplit[i]);
+					c.m_appliedPushImpulse = m_xSplit[i];
+				}
+				c.m_appliedImpulse = m_x[i];
+			}
+		}
+	}
+	else
+	{
+		m_fallback++;
+		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+	}
+
+	return 0.f;
+}

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h

@@ -0,0 +1,81 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-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.
+*/
+///original version written by Erwin Coumans, October 2013
+
+#ifndef BT_MLCP_SOLVER_H
+#define BT_MLCP_SOLVER_H
+
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "LinearMath/btMatrixX.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
+
+class btMLCPSolver : public btSequentialImpulseConstraintSolver
+{
+
+protected:
+	
+	btMatrixXu m_A;
+	btVectorXu m_b;
+	btVectorXu m_x;
+	btVectorXu m_lo;
+	btVectorXu m_hi;
+	
+	///when using 'split impulse' we solve two separate (M)LCPs
+	btVectorXu m_bSplit;
+	btVectorXu m_xSplit;
+	btVectorXu m_bSplit1;
+	btVectorXu m_xSplit2;
+
+	btAlignedObjectArray<int> m_limitDependencies;
+	btConstraintArray m_allConstraintArray;
+	btMLCPSolverInterface* m_solver;
+	int m_fallback;
+
+	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 void createMLCP(const btContactSolverInfo& infoGlobal);
+	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
+
+	//return true is it solves the problem successfully
+	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
+
+public:
+
+	btMLCPSolver(	 btMLCPSolverInterface* solver);
+	virtual ~btMLCPSolver();
+
+	void setMLCPSolver(btMLCPSolverInterface* solver)
+	{
+		m_solver = solver;
+	}
+
+	int getNumFallbacks() const
+	{
+		return m_fallback;
+	}
+	void setNumFallbacks(int num)
+	{
+		m_fallback = num;
+	}
+
+	virtual btConstraintSolverType	getSolverType() const
+	{
+		return BT_MLCP_SOLVER;
+	}
+
+};
+
+
+#endif //BT_MLCP_SOLVER_H

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h

@@ -0,0 +1,33 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-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.
+*/
+///original version written by Erwin Coumans, October 2013
+
+#ifndef BT_MLCP_SOLVER_INTERFACE_H
+#define BT_MLCP_SOLVER_INTERFACE_H
+
+#include "LinearMath/btMatrixX.h"
+
+class btMLCPSolverInterface
+{
+public:
+	virtual ~btMLCPSolverInterface()
+	{
+	}
+
+	//return true is it solves the problem successfully
+	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)=0;
+};
+
+#endif //BT_MLCP_SOLVER_INTERFACE_H

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btPATHSolver.h

@@ -0,0 +1,151 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-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.
+*/
+///original version written by Erwin Coumans, October 2013
+
+
+#ifndef BT_PATH_SOLVER_H
+#define BT_PATH_SOLVER_H
+
+//#define BT_USE_PATH
+#ifdef BT_USE_PATH
+
+extern "C" {
+#include "PATH/SimpleLCP.h"
+#include "PATH/License.h"
+#include "PATH/Error_Interface.h"
+};
+  void __stdcall MyError(Void *data, Char *msg)
+{
+	printf("Path Error: %s\n",msg);
+}
+  void __stdcall MyWarning(Void *data, Char *msg)
+{
+	printf("Path Warning: %s\n",msg);
+}
+
+Error_Interface e;
+
+
+
+#include "btMLCPSolverInterface.h"
+#include "Dantzig/lcp.h"
+
+class btPathSolver : public btMLCPSolverInterface
+{
+public:
+
+	btPathSolver()
+	{
+		License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0");
+		e.error_data = 0;
+		e.warning = MyWarning;
+		e.error = MyError;
+		Error_SetInterface(&e);
+	}
+
+
+	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	{
+		MCP_Termination status;
+		
+
+		int numVariables = b.rows();
+		if (0==numVariables)
+			return true;
+
+			/*	 - variables - the number of variables in the problem
+			- m_nnz - the number of nonzeros in the M matrix
+			- m_i - a vector of size m_nnz containing the row indices for M
+			- m_j - a vector of size m_nnz containing the column indices for M
+			- m_ij - a vector of size m_nnz containing the data for M
+			- q - a vector of size variables
+			- lb - a vector of size variables containing the lower bounds on x
+			- ub - a vector of size variables containing the upper bounds on x
+			*/
+		btAlignedObjectArray<double> values;
+		btAlignedObjectArray<int> rowIndices;
+		btAlignedObjectArray<int> colIndices;
+
+		for (int i=0;i<A.rows();i++)
+		{
+			for (int j=0;j<A.cols();j++)
+			{
+				if (A(i,j)!=0.f)
+				{
+					//add 1, because Path starts at 1, instead of 0
+					rowIndices.push_back(i+1);
+					colIndices.push_back(j+1);
+					values.push_back(A(i,j));
+				}
+			}
+		}
+		int numNonZero = rowIndices.size();
+		btAlignedObjectArray<double> zResult;
+		zResult.resize(numVariables);
+		btAlignedObjectArray<double> rhs;
+		btAlignedObjectArray<double> upperBounds;
+		btAlignedObjectArray<double> lowerBounds;
+		for (int i=0;i<numVariables;i++)
+		{
+			upperBounds.push_back(hi[i]);
+			lowerBounds.push_back(lo[i]);
+			rhs.push_back(-b[i]);
+		}
+
+
+		SimpleLCP(numVariables,numNonZero,&rowIndices[0],&colIndices[0],&values[0],&rhs[0],&lowerBounds[0],&upperBounds[0], &status, &zResult[0]);
+
+		if (status != MCP_Solved)
+		{
+			static const char* gReturnMsgs[] = {
+				"Invalid return",
+				"MCP_Solved: The problem was solved",
+				"MCP_NoProgress: A stationary point was found",
+				"MCP_MajorIterationLimit: Major iteration limit met",
+				"MCP_MinorIterationLimit: Cumulative minor iteration limit met",
+				"MCP_TimeLimit: Ran out of time",
+				"MCP_UserInterrupt: Control-C, typically",
+				"MCP_BoundError: Problem has a bound error",
+				"MCP_DomainError: Could not find starting point",
+				"MCP_Infeasible: Problem has no solution",
+				"MCP_Error: An error occurred within the code",
+				"MCP_LicenseError: License could not be found",
+				"MCP_OK"
+			};
+
+			printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
+			printf("using Projected Gauss Seidel fallback\n");
+			
+			return false;
+		} else
+		{
+			for (int i=0;i<numVariables;i++)
+			{
+				x[i] = zResult[i];
+				//check for #NAN
+				if (x[i] != zResult[i])
+					return false;
+			}
+			return true;
+
+		}
+
+	}
+};
+
+#endif //BT_USE_PATH
+
+
+#endif //BT_PATH_SOLVER_H

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h

@@ -0,0 +1,80 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-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.
+*/
+///original version written by Erwin Coumans, October 2013
+
+#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
+#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
+
+
+#include "btMLCPSolverInterface.h"
+
+class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
+{
+public:
+	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	{
+		//A is a m-n matrix, m rows, n columns
+		btAssert(A.rows() == b.rows());
+
+		int i, j, numRows = A.rows();
+	
+		float delta;
+
+		for (int k = 0; k <numIterations; k++)
+		{
+			for (i = 0; i <numRows; i++)
+			{
+				delta = 0.0f;
+				if (useSparsity)
+				{
+					for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
+					{
+						int j = A.m_rowNonZeroElements1[i][h];
+						if (j != i)//skip main diagonal
+						{
+							delta += A(i,j) * x[j];
+						}
+					}
+				} else
+				{
+					for (j = 0; j <i; j++) 
+						delta += A(i,j) * x[j];
+					for (j = i+1; j<numRows; j++) 
+						delta += A(i,j) * x[j];
+				}
+
+				float aDiag = A(i,i);
+				x [i] = (b [i] - delta) / A(i,i);
+				float s = 1.f;
+
+				if (limitDependency[i]>=0)
+				{
+					s = x[limitDependency[i]];
+					if (s<0)
+						s=1;
+				}
+			
+				if (x[i]<lo[i]*s)
+					x[i]=lo[i]*s;
+				if (x[i]>hi[i]*s)
+					x[i]=hi[i]*s;
+			}
+		}
+		return true;
+	}
+
+};
+
+#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H

Source/ThirdParty/Bullet/src/BulletSoftBody/btSoftBodyHelpers.cpp

@@ -911,9 +911,9 @@ btSoftBody*		btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldI
 		&hres.m_OutputVertices[0],0);
 	for(int i=0;i<(int)hres.mNumFaces;++i)
 	{
-		const int idx[]={	hres.m_Indices[i*3+0],
-			hres.m_Indices[i*3+1],
-			hres.m_Indices[i*3+2]};
+		const int idx[]={	static_cast<int>(hres.m_Indices[i*3+0]),
+							static_cast<int>(hres.m_Indices[i*3+1]),
+							static_cast<int>(hres.m_Indices[i*3+2])};
 		if(idx[0]<idx[1]) psb->appendLink(	idx[0],idx[1]);
 		if(idx[1]<idx[2]) psb->appendLink(	idx[1],idx[2]);
 		if(idx[2]<idx[0]) psb->appendLink(	idx[2],idx[0]);

Source/ThirdParty/Bullet/src/LinearMath/btIDebugDraw.h

@@ -66,29 +66,17 @@ class	btIDebugDraw
 
 	virtual void	drawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
 	{
-		btVector3 start = transform.getOrigin();
-
-		const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
-		const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
-		const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
-
-		// XY 
-		drawLine(start-xoffs, start+yoffs, color);
-		drawLine(start+yoffs, start+xoffs, color);
-		drawLine(start+xoffs, start-yoffs, color);
-		drawLine(start-yoffs, start-xoffs, color);
-
-		// XZ
-		drawLine(start-xoffs, start+zoffs, color);
-		drawLine(start+zoffs, start+xoffs, color);
-		drawLine(start+xoffs, start-zoffs, color);
-		drawLine(start-zoffs, start-xoffs, color);
-
-		// YZ
-		drawLine(start-yoffs, start+zoffs, color);
-		drawLine(start+zoffs, start+yoffs, color);
-		drawLine(start+yoffs, start-zoffs, color);
-		drawLine(start-zoffs, start-yoffs, color);
+		
+		btVector3 center = transform.getOrigin();
+		btVector3 up = transform.getBasis().getColumn(1);
+		btVector3 axis = transform.getBasis().getColumn(0);
+		btScalar minTh = -SIMD_HALF_PI;
+		btScalar maxTh = SIMD_HALF_PI;
+		btScalar minPs = -SIMD_HALF_PI;
+		btScalar maxPs = SIMD_HALF_PI;
+		btScalar stepDegrees = 30.f;
+		drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees ,false);
+		drawSpherePatch(center, up, -axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees,false );
 	}
 	
 	virtual void	drawSphere (const btVector3& p, btScalar radius, const btVector3& color)
@@ -183,7 +171,7 @@ class	btIDebugDraw
 		}
 	}
 	virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius, 
-		btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f))
+		btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f),bool drawCenter = true)
 	{
 		btVector3 vA[74];
 		btVector3 vB[74];
@@ -265,18 +253,22 @@ class	btIDebugDraw
 				{
 					drawLine(npole, pvB[j], color);
 				}
-				if(isClosed)
+				
+				if (drawCenter)
 				{
-					if(j == (n_vert-1))
+					if(isClosed)
 					{
-						drawLine(arcStart, pvB[j], color);
+						if(j == (n_vert-1))
+						{
+							drawLine(arcStart, pvB[j], color);
+						}
 					}
-				}
-				else
-				{
-					if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1))))
+					else
 					{
-						drawLine(center, pvB[j], color);
+						if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1))))
+						{
+							drawLine(center, pvB[j], color);
+						}
 					}
 				}
 			}
@@ -318,6 +310,8 @@ class	btIDebugDraw
 
 	virtual void drawCapsule(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color)
 	{
+		int stepDegrees = 30;
+
 		btVector3 capStart(0.f,0.f,0.f);
 		capStart[upAxis] = -halfHeight;
 
@@ -329,34 +323,47 @@ class	btIDebugDraw
 
 			btTransform childTransform = transform;
 			childTransform.getOrigin() = transform * capStart;
-			drawSphere(radius, childTransform, color);
+			{
+				btVector3 center = childTransform.getOrigin();
+				btVector3 up = childTransform.getBasis().getColumn((upAxis+1)%3);
+				btVector3 axis = -childTransform.getBasis().getColumn(upAxis);
+				btScalar minTh = -SIMD_HALF_PI;
+				btScalar maxTh = SIMD_HALF_PI;
+				btScalar minPs = -SIMD_HALF_PI;
+				btScalar maxPs = SIMD_HALF_PI;
+				
+				drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees) ,false);
+			}
+
+
+
 		}
 
 		{
 			btTransform childTransform = transform;
 			childTransform.getOrigin() = transform * capEnd;
-			drawSphere(radius, childTransform, color);
+			{
+				btVector3 center = childTransform.getOrigin();
+				btVector3 up = childTransform.getBasis().getColumn((upAxis+1)%3);
+				btVector3 axis = childTransform.getBasis().getColumn(upAxis);
+				btScalar minTh = -SIMD_HALF_PI;
+				btScalar maxTh = SIMD_HALF_PI;
+				btScalar minPs = -SIMD_HALF_PI;
+				btScalar maxPs = SIMD_HALF_PI;
+				drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees) ,false);
+			}
 		}
 
 		// Draw some additional lines
 		btVector3 start = transform.getOrigin();
 
-		capStart[(upAxis+1)%3] = radius;
-		capEnd[(upAxis+1)%3] = radius;
-		drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
-		capStart[(upAxis+1)%3] = -radius;
-		capEnd[(upAxis+1)%3] = -radius;
-		drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
-
-		capStart[(upAxis+1)%3] = 0.f;
-		capEnd[(upAxis+1)%3] = 0.f;
-
-		capStart[(upAxis+2)%3] = radius;
-		capEnd[(upAxis+2)%3] = radius;
-		drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
-		capStart[(upAxis+2)%3] = -radius;
-		capEnd[(upAxis+2)%3] = -radius;
-		drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
+		for (int i=0;i<360;i+=stepDegrees)
+		{
+			capEnd[(upAxis+1)%3] = capStart[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
+			capEnd[(upAxis+2)%3] = capStart[(upAxis+2)%3]  = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
+			drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
+		}
+		
 	}
 
 	virtual void drawCylinder(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color)
@@ -364,11 +371,18 @@ class	btIDebugDraw
 		btVector3 start = transform.getOrigin();
 		btVector3	offsetHeight(0,0,0);
 		offsetHeight[upAxis] = halfHeight;
-		btVector3	offsetRadius(0,0,0);
-		offsetRadius[(upAxis+1)%3] = radius;
-		drawLine(start+transform.getBasis() * (offsetHeight+offsetRadius),start+transform.getBasis() * (-offsetHeight+offsetRadius),color);
-		drawLine(start+transform.getBasis() * (offsetHeight-offsetRadius),start+transform.getBasis() * (-offsetHeight-offsetRadius),color);
+		int stepDegrees=30;
+		btVector3 capStart(0.f,0.f,0.f);
+		capStart[upAxis] = -halfHeight;
+		btVector3 capEnd(0.f,0.f,0.f);
+		capEnd[upAxis] = halfHeight;
 
+		for (int i=0;i<360;i+=stepDegrees)
+		{
+			capEnd[(upAxis+1)%3] = capStart[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
+			capEnd[(upAxis+2)%3] = capStart[(upAxis+2)%3]  = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
+			drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
+		}
 		// Drawing top and bottom caps of the cylinder
 		btVector3 yaxis(0,0,0);
 		yaxis[upAxis] = btScalar(1.0);
@@ -380,16 +394,28 @@ class	btIDebugDraw
 
 	virtual void drawCone(btScalar radius, btScalar height, int upAxis, const btTransform& transform, const btVector3& color)
 	{
-
+		int stepDegrees = 30;
 		btVector3 start = transform.getOrigin();
 
 		btVector3	offsetHeight(0,0,0);
-		offsetHeight[upAxis] = height * btScalar(0.5);
+		btScalar halfHeight = height * btScalar(0.5);
+		offsetHeight[upAxis] = halfHeight;
 		btVector3	offsetRadius(0,0,0);
 		offsetRadius[(upAxis+1)%3] = radius;
 		btVector3	offset2Radius(0,0,0);
 		offset2Radius[(upAxis+2)%3] = radius;
 
+
+		btVector3 capEnd(0.f,0.f,0.f);
+		capEnd[upAxis] = -halfHeight;
+
+		for (int i=0;i<360;i+=stepDegrees)
+		{
+			capEnd[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
+			capEnd[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
+			drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * capEnd, color);
+		}
+
 		drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offsetRadius),color);
 		drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offsetRadius),color);
 		drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offset2Radius),color);

Source/ThirdParty/Bullet/src/LinearMath/btMatrix3x3.h

@@ -212,7 +212,7 @@ public:
 		btFullAssert(d != btScalar(0.0));
 		btScalar s = btScalar(2.0) / d;
     
-    #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+    #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
         __m128	vs, Q = q.get128();
 		__m128i Qi = btCastfTo128i(Q);
         __m128	Y, Z;
@@ -346,7 +346,7 @@ public:
 	* @param m The array to be filled */
 	void getOpenGLSubMatrix(btScalar *m) const 
 	{
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
         __m128 v0 = m_el[0].mVec128;
         __m128 v1 = m_el[1].mVec128;
         __m128 v2 = m_el[2].mVec128;    //  x2 y2 z2 w2
@@ -367,7 +367,7 @@ public:
         vm[2] = v2;
 #elif defined(BT_USE_NEON)
         // note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
-        static const uint32x2_t zMask = (const uint32x2_t) {-1, 0 };
+        static const uint32x2_t zMask = (const uint32x2_t) {static_cast<uint32_t>(-1), 0 };
         float32x4_t *vm = (float32x4_t *)m;
         float32x4x2_t top = vtrnq_f32( m_el[0].mVec128, m_el[1].mVec128 );  // {x0 x1 z0 z1}, {y0 y1 w0 w1}
         float32x2x2_t bl = vtrn_f32( vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f) );       // {x2  0 }, {y2 0}
@@ -745,7 +745,7 @@ public:
 SIMD_FORCE_INLINE btMatrix3x3& 
 btMatrix3x3::operator*=(const btMatrix3x3& m)
 {
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
     __m128 rv00, rv01, rv02;
     __m128 rv10, rv11, rv12;
     __m128 rv20, rv21, rv22;
@@ -958,7 +958,7 @@ btMatrix3x3::determinant() const
 SIMD_FORCE_INLINE btMatrix3x3 
 btMatrix3x3::absolute() const
 {
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
+#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
     return btMatrix3x3(
             _mm_and_ps(m_el[0].mVec128, btvAbsfMask),
             _mm_and_ps(m_el[1].mVec128, btvAbsfMask),
@@ -979,7 +979,7 @@ btMatrix3x3::absolute() const
 SIMD_FORCE_INLINE btMatrix3x3 
 btMatrix3x3::transpose() const 
 {
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
+#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
     __m128 v0 = m_el[0].mVec128;
     __m128 v1 = m_el[1].mVec128;
     __m128 v2 = m_el[2].mVec128;    //  x2 y2 z2 w2
@@ -998,7 +998,7 @@ btMatrix3x3::transpose() const
     return btMatrix3x3( v0, v1, v2 );
 #elif defined(BT_USE_NEON)
     // note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
-    static const uint32x2_t zMask = (const uint32x2_t) {-1, 0 };
+    static const uint32x2_t zMask = (const uint32x2_t) {static_cast<uint32_t>(-1), 0 };
     float32x4x2_t top = vtrnq_f32( m_el[0].mVec128, m_el[1].mVec128 );  // {x0 x1 z0 z1}, {y0 y1 w0 w1}
     float32x2x2_t bl = vtrn_f32( vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f) );       // {x2  0 }, {y2 0}
     float32x4_t v0 = vcombine_f32( vget_low_f32(top.val[0]), bl.val[0] );
@@ -1036,7 +1036,7 @@ btMatrix3x3::inverse() const
 SIMD_FORCE_INLINE btMatrix3x3 
 btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
 {
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
+#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
     // zeros w
 //    static const __m128i xyzMask = (const __m128i){ -1ULL, 0xffffffffULL };
     __m128 row = m_el[0].mVec128;
@@ -1058,7 +1058,7 @@ btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
 
 #elif defined BT_USE_NEON
     // zeros w
-    static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 };
+    static const uint32x4_t xyzMask = (const uint32x4_t){ static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), 0 };
     float32x4_t m0 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(0).mVec128, xyzMask );
     float32x4_t m1 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(1).mVec128, xyzMask );
     float32x4_t m2 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(2).mVec128, xyzMask );
@@ -1156,7 +1156,7 @@ operator*(const btMatrix3x3& m, const btVector3& v)
 SIMD_FORCE_INLINE btVector3
 operator*(const btVector3& v, const btMatrix3x3& m)
 {
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
+#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
 
     const __m128 vv = v.mVec128;
 
@@ -1196,7 +1196,7 @@ operator*(const btVector3& v, const btMatrix3x3& m)
 SIMD_FORCE_INLINE btMatrix3x3 
 operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
 {
-#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
+#if defined BT_USE_SIMD_VECTOR3 && (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
 
     __m128 m10 = m1[0].mVec128;  
     __m128 m11 = m1[1].mVec128;

Source/ThirdParty/Bullet/src/LinearMath/btMatrixX.h

@@ -0,0 +1,504 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-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.
+*/
+///original version written by Erwin Coumans, October 2013
+
+#ifndef BT_MATRIX_X_H
+#define BT_MATRIX_X_H
+
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+class btIntSortPredicate
+{
+	public:
+		bool operator() ( const int& a, const int& b ) const
+		{
+			 return a < b;
+		}
+};
+
+
+template <typename T> 
+struct btMatrixX
+{
+	int m_rows;
+	int m_cols;
+	int m_operations;
+	int m_resizeOperations;
+	int m_setElemOperations;
+
+	btAlignedObjectArray<T>	m_storage;
+	btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
+	btAlignedObjectArray< btAlignedObjectArray<int> > m_colNonZeroElements;
+
+	T* getBufferPointerWritable() 
+	{
+		return m_storage.size() ? &m_storage[0] : 0;
+	}
+
+	const T* getBufferPointer() const
+	{
+		return m_storage.size() ? &m_storage[0] : 0;
+	}
+	btMatrixX()
+		:m_rows(0),
+		m_cols(0),
+		m_operations(0),
+		m_resizeOperations(0),
+		m_setElemOperations(0)
+	{
+	}
+	btMatrixX(int rows,int cols)
+		:m_rows(rows),
+		m_cols(cols),
+		m_operations(0),
+		m_resizeOperations(0),
+		m_setElemOperations(0)
+	{
+		resize(rows,cols);
+	}
+	void resize(int rows, int cols)
+	{
+		m_resizeOperations++;
+		m_rows = rows;
+		m_cols = cols;
+		{
+			BT_PROFILE("m_storage.resize");
+			m_storage.resize(rows*cols);
+		}
+		clearSparseInfo();
+	}
+	int cols() const
+	{
+		return m_cols;
+	}
+	int rows() const
+	{
+		return m_rows;
+	}
+	///we don't want this read/write operator(), because we cannot keep track of non-zero elements, use setElem instead
+	/*T& operator() (int row,int col)
+	{
+		return m_storage[col*m_rows+row];
+	}
+	*/
+
+	void addElem(int row,int col, T val)
+	{
+		if (val)
+		{
+			if (m_storage[col+row*m_cols]==0.f)
+			{
+				setElem(row,col,val);
+			} else
+			{
+				m_storage[row*m_cols+col] += val;
+			}
+		}
+	}
+	
+	void copyLowerToUpperTriangle()
+	{
+		int count=0;
+		for (int row=0;row<m_rowNonZeroElements1.size();row++)
+		{
+			for (int j=0;j<m_rowNonZeroElements1[row].size();j++)
+			{
+				int col = m_rowNonZeroElements1[row][j];
+				setElem(col,row, (*this)(row,col));
+				count++;
+
+			}
+		}
+		//printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
+	}
+	void setElem(int row,int col, T val)
+	{
+		m_setElemOperations++;
+		if (val)
+		{
+			if (m_storage[col+row*m_cols]==0.f)
+			{
+				m_rowNonZeroElements1[row].push_back(col);
+				m_colNonZeroElements[col].push_back(row);
+			}
+			m_storage[row*m_cols+col] = val;
+		}
+	}
+	const T& operator() (int row,int col) const
+	{
+		return m_storage[col+row*m_cols];
+	}
+
+	void clearSparseInfo()
+	{
+		BT_PROFILE("clearSparseInfo=0");
+		m_rowNonZeroElements1.resize(m_rows);
+		m_colNonZeroElements.resize(m_cols);
+		for (int i=0;i<m_rows;i++)
+			m_rowNonZeroElements1[i].resize(0);
+		for (int j=0;j<m_cols;j++)
+			m_colNonZeroElements[j].resize(0);
+	}
+
+	void setZero()
+	{
+		{
+			BT_PROFILE("storage=0");
+			btSetZero(&m_storage[0],m_storage.size());
+			//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
+			//for (int i=0;i<m_storage.size();i++)
+	//			m_storage[i]=0;
+		}
+		{
+			BT_PROFILE("clearSparseInfo=0");
+			clearSparseInfo();
+		}
+	}
+
+	void	printMatrix(const char* msg)
+	{
+		printf("%s ---------------------\n",msg);
+		for (int i=0;i<rows();i++)
+		{
+			printf("\n");
+			for (int j=0;j<cols();j++)
+			{
+				printf("%2.1f\t",(*this)(i,j));
+			}
+		}
+		printf("\n---------------------\n");
+
+	}
+	void	printNumZeros(const char* msg)
+	{
+		printf("%s: ",msg);
+		int numZeros = 0;
+		for (int i=0;i<m_storage.size();i++)
+			if (m_storage[i]==0)
+				numZeros++;
+		int total = m_cols*m_rows;
+		int computedNonZero = total-numZeros;
+		int nonZero = 0;
+		for (int i=0;i<m_colNonZeroElements.size();i++)
+			nonZero += m_colNonZeroElements[i].size();
+		btAssert(computedNonZero==nonZero);
+		if(computedNonZero!=nonZero)
+		{
+			printf("Error: computedNonZero=%d, but nonZero=%d\n",computedNonZero,nonZero);
+		}
+		//printf("%d numZeros out of %d (%f)\n",numZeros,m_cols*m_rows,numZeros/(m_cols*m_rows));
+		printf("total %d, %d rows, %d cols, %d non-zeros (%f %)\n", total, rows(),cols(), nonZero,100.f*(T)nonZero/T(total));
+	}
+	/*
+	void rowComputeNonZeroElements()
+	{
+		m_rowNonZeroElements1.resize(rows());
+		for (int i=0;i<rows();i++)
+		{
+			m_rowNonZeroElements1[i].resize(0);
+			for (int j=0;j<cols();j++)
+			{
+				if ((*this)(i,j)!=0.f)
+				{
+					m_rowNonZeroElements1[i].push_back(j);
+				}
+			}
+		}
+	}
+	*/
+	btMatrixX transpose() const
+	{
+		//transpose is optimized for sparse matrices
+		btMatrixX tr(m_cols,m_rows);
+		tr.setZero();
+#if 0
+		for (int i=0;i<m_cols;i++)
+			for (int j=0;j<m_rows;j++)
+			{
+				T v = (*this)(j,i);
+				if (v)
+				{
+					tr.setElem(i,j,v);
+				}
+			}
+#else		
+		for (int i=0;i<m_colNonZeroElements.size();i++)
+			for (int h=0;h<m_colNonZeroElements[i].size();h++)
+			{
+				int j = m_colNonZeroElements[i][h];
+				T v = (*this)(j,i);
+				tr.setElem(i,j,v);
+			}
+#endif
+		return tr;
+	}
+
+	void sortRowIndexArrays()
+	{
+		for (int i=0;i<m_rowNonZeroElements1[i].size();i++)
+		{
+			m_rowNonZeroElements1[i].quickSort(btIntSortPredicate());
+		}
+	}
+
+	void sortColIndexArrays()
+	{
+		for (int i=0;i<m_colNonZeroElements[i].size();i++)
+		{
+			m_colNonZeroElements[i].quickSort(btIntSortPredicate());
+		}
+	}
+
+	btMatrixX operator*(const btMatrixX& other)
+	{
+		//btMatrixX*btMatrixX implementation, optimized for sparse matrices
+		btAssert(cols() == other.rows());
+
+		btMatrixX res(rows(),other.cols());
+		res.setZero();
+//		BT_PROFILE("btMatrixX mul");
+		for (int j=0; j < res.cols(); ++j)
+		{
+			//int numZero=other.m_colNonZeroElements[j].size();
+			//if (numZero)
+			{
+				for (int i=0; i < res.rows(); ++i)
+				//for (int g = 0;g<m_colNonZeroElements[j].size();g++)
+				{
+					T dotProd=0;
+					T dotProd2=0;
+					int waste=0,waste2=0;
+
+					bool doubleWalk = false;
+					if (doubleWalk)
+					{
+						int numRows = m_rowNonZeroElements1[i].size();
+						int numOtherCols = other.m_colNonZeroElements[j].size();
+						for (int ii=0;ii<numRows;ii++)
+						{
+							int vThis=m_rowNonZeroElements1[i][ii];
+						}
+
+						for (int ii=0;ii<numOtherCols;ii++)
+						{
+							int vOther = other.m_colNonZeroElements[j][ii];
+						}
+
+
+						int indexRow = 0;
+						int indexOtherCol = 0;
+						while (indexRow < numRows && indexOtherCol < numOtherCols)
+						{
+							int vThis=m_rowNonZeroElements1[i][indexRow];
+							int vOther = other.m_colNonZeroElements[j][indexOtherCol];
+							if (vOther==vThis)
+							{
+								dotProd += (*this)(i,vThis) * other(vThis,j);
+							}
+							if (vThis<vOther)
+							{
+								indexRow++;
+							} else
+							{
+								indexOtherCol++;
+							}
+						}
+
+					} else
+					{
+						bool useOtherCol = true;
+						if (other.m_colNonZeroElements[j].size() <m_rowNonZeroElements1[i].size())
+						{
+						useOtherCol=true;
+						}
+						if (!useOtherCol )
+						{
+							for (int q=0;q<other.m_colNonZeroElements[j].size();q++)
+							{
+								int v = other.m_colNonZeroElements[j][q];
+								T w = (*this)(i,v);
+								if (w!=0.f)
+								{
+									dotProd+=w*other(v,j);
+								}
+						
+							}
+						}
+						else
+						{
+							for (int q=0;q<m_rowNonZeroElements1[i].size();q++)
+							{
+								int v=m_rowNonZeroElements1[i][q];
+								T w = (*this)(i,v);
+								if (other(v,j)!=0.f)
+								{
+									dotProd+=w*other(v,j);	
+								}
+						
+							}
+						}
+					}
+					if (dotProd)
+						res.setElem(i,j,dotProd);
+				}
+			}
+		}
+		return res;
+	}
+
+	// this assumes the 4th and 8th rows of B and C are zero.
+	void multiplyAdd2_p8r (const btScalar *B, const btScalar *C,  int numRows,  int numRowsOther ,int row, int col)
+	{
+		const btScalar *bb = B;
+		for ( int i = 0;i<numRows;i++)
+		{
+			const btScalar *cc = C;
+			for ( int j = 0;j<numRowsOther;j++)
+			{
+				btScalar sum;
+				sum  = bb[0]*cc[0];
+				sum += bb[1]*cc[1];
+				sum += bb[2]*cc[2];
+				sum += bb[4]*cc[4];
+				sum += bb[5]*cc[5];
+				sum += bb[6]*cc[6];
+				addElem(row+i,col+j,sum);
+				cc += 8;
+			}
+			bb += 8;
+		}
+	}
+
+	void multiply2_p8r (const btScalar *B, const btScalar *C,  int numRows,  int numRowsOther, int row, int col)
+	{
+		btAssert (numRows>0 && numRowsOther>0 && B && C);
+		const btScalar *bb = B;
+		for ( int i = 0;i<numRows;i++)
+		{
+			const btScalar *cc = C;
+			for ( int j = 0;j<numRowsOther;j++)
+			{
+				btScalar sum;
+				sum  = bb[0]*cc[0];
+				sum += bb[1]*cc[1];
+				sum += bb[2]*cc[2];
+				sum += bb[4]*cc[4];
+				sum += bb[5]*cc[5];
+				sum += bb[6]*cc[6];
+				setElem(row+i,col+j,sum);
+				cc += 8;
+			}
+			bb += 8;
+		}
+	}
+
+};
+
+template <typename T> 
+struct btVectorX
+{
+	btAlignedObjectArray<T>	m_storage;
+
+	btVectorX()
+	{
+	}
+	btVectorX(int numRows)
+	{
+		m_storage.resize(numRows);
+	}
+
+	void resize(int rows)
+	{
+		m_storage.resize(rows);
+	}
+	int cols() const
+	{
+		return 1;
+	}
+	int rows() const
+	{
+		return m_storage.size();
+	}
+	int size() const
+	{
+		return rows();
+	}
+	void	setZero()
+	{
+	//	for (int i=0;i<m_storage.size();i++)
+	//		m_storage[i]=0;
+		//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
+		btSetZero(&m_storage[0],m_storage.size());
+	}
+	const T& operator[] (int index) const
+	{
+		return m_storage[index];
+	}
+
+	T& operator[] (int index)
+	{
+		return m_storage[index];
+	}
+
+	T* getBufferPointerWritable() 
+	{
+		return m_storage.size() ? &m_storage[0] : 0;
+	}
+
+	const T* getBufferPointer() const
+	{
+		return m_storage.size() ? &m_storage[0] : 0;
+	}
+
+};
+/*
+template <typename T> 
+void setElem(btMatrixX<T>& mat, int row, int col, T val)
+{
+	mat.setElem(row,col,val);
+}
+*/
+
+
+typedef btMatrixX<float> btMatrixXf;
+typedef btVectorX<float> btVectorXf;
+
+typedef btMatrixX<double> btMatrixXd;
+typedef btVectorX<double> btVectorXd;
+
+
+
+inline void setElem(btMatrixXd& mat, int row, int col, double val)
+{
+	mat.setElem(row,col,val);
+}
+
+inline void setElem(btMatrixXf& mat, int row, int col, float val)
+{
+	mat.setElem(row,col,val);
+}
+
+#ifdef BT_USE_DOUBLE_PRECISION
+	#define btVectorXu btVectorXd
+	#define btMatrixXu btMatrixXd
+#else
+	#define btVectorXu btVectorXf
+	#define btMatrixXu btMatrixXf
+#endif //BT_USE_DOUBLE_PRECISION
+
+
+
+#endif//BT_MATRIX_H_H

Source/ThirdParty/Bullet/src/LinearMath/btQuaternion.h

@@ -291,7 +291,7 @@ public:
    * @param q The other quaternion */
 	btScalar dot(const btQuaternion& q) const
 	{
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
 		__m128	vd;
 		
 		vd = _mm_mul_ps(mVec128, q.mVec128);
@@ -390,7 +390,7 @@ public:
 	{
 		return *this / length();
 	} 
-  /**@brief Return the angle between this quaternion and the other 
+	/**@brief Return the ***half*** angle between this quaternion and the other
    * @param q The other quaternion */
 	btScalar angle(const btQuaternion& q) const 
 	{
@@ -398,6 +398,19 @@ public:
 		btAssert(s != btScalar(0.0));
 		return btAcos(dot(q) / s);
 	}
+	
+	/**@brief Return the angle between this quaternion and the other along the shortest path
+	* @param q The other quaternion */
+	btScalar angleShortestPath(const btQuaternion& q) const 
+	{
+		btScalar s = btSqrt(length2() * q.length2());
+		btAssert(s != btScalar(0.0));
+		if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
+			return btAcos(dot(-q) / s) * btScalar(2.0);
+		else 
+			return btAcos(dot(q) / s) * btScalar(2.0);
+	}
+
   /**@brief Return the angle of rotation represented by this quaternion */
 	btScalar getAngle() const 
 	{
@@ -405,6 +418,19 @@ public:
 		return s;
 	}
 
+	/**@brief Return the angle of rotation represented by this quaternion along the shortest path*/
+	btScalar getAngleShortestPath() const 
+	{
+		btScalar s;
+		if (dot(*this) < 0)
+			s = btScalar(2.) * btAcos(m_floats[3]);
+		else
+			s = btScalar(2.) * btAcos(-m_floats[3]);
+
+		return s;
+	}
+
+
 	/**@brief Return the axis of the rotation represented by this quaternion */
 	btVector3 getAxis() const
 	{
@@ -841,7 +867,7 @@ quatRotate(const btQuaternion& rotation, const btVector3& v)
 {
 	btQuaternion q = rotation * v;
 	q *= rotation.inverse();
-#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
+#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
 	return btVector3(_mm_and_ps(q.get128(), btvFFF0fMask));
 #elif defined(BT_USE_NEON)
     return btVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), btvFFF0Mask));

Source/ThirdParty/Bullet/src/LinearMath/btScalar.h

@@ -14,6 +14,7 @@ subject to the following restrictions:
 
 // Modified by Lasse Oorni for Urho3D
 
+
 #ifndef BT_SCALAR_H
 #define BT_SCALAR_H
 
@@ -28,7 +29,7 @@ subject to the following restrictions:
 #include <float.h>
 
 /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
-#define BT_BULLET_VERSION 281
+#define BT_BULLET_VERSION 282
 
 inline int	btGetVersion()
 {
@@ -182,7 +183,7 @@ inline int	btGetVersion()
                 #include <emmintrin.h>
             #endif
         #endif //BT_USE_SSE
-    #elif defined( __armv7__ )
+    #elif defined( __ARM_NEON__ )
         #ifdef __clang__
             #define BT_USE_NEON 1
 			#define BT_USE_SIMD_VECTOR3
@@ -259,10 +260,12 @@ inline int	btGetVersion()
 
 ///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
 #if defined(BT_USE_DOUBLE_PRECISION)
+
 typedef double btScalar;
 //this number could be bigger in double precision
 #define BT_LARGE_FLOAT 1e30
 #else
+
 typedef float btScalar;
 //keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
 #define BT_LARGE_FLOAT 1e18f
@@ -286,6 +289,8 @@ static  int btInfinityMask = 0x7F800000;
 #define BT_INFINITY (*(float*)&btInfinityMask)
 #endif
 
+//use this, in case there are clashes (such as xnamath.h)
+#ifndef BT_NO_SIMD_OPERATOR_OVERLOADS
 inline __m128 operator + (const __m128 A, const __m128 B)
 {
     return _mm_add_ps(A, B);
@@ -300,6 +305,7 @@ inline __m128 operator * (const __m128 A, const __m128 B)
 {
     return _mm_mul_ps(A, B);
 }
+#endif //BT_NO_SIMD_OPERATOR_OVERLOADS
 
 #define btCastfTo128i(a) (_mm_castps_si128(a))
 #define btCastfTo128d(a) (_mm_castps_pd(a))
@@ -319,7 +325,24 @@ inline __m128 operator * (const __m128 A, const __m128 B)
 #define BT_INFINITY INFINITY
 #define BT_NAN NAN
 #endif//_WIN32
-#endif //BT_USE_SSE_IN_API
+#else
+
+#ifdef BT_USE_NEON
+	#include <arm_neon.h>
+
+	typedef float32x4_t btSimdFloat4;
+	#define BT_INFINITY INFINITY
+	#define BT_NAN NAN
+	#define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
+#else//BT_USE_NEON
+
+	#ifndef BT_INFINITY
+	static  int btInfinityMask = 0x7F800000;
+	#define BT_INFINITY (*(float*)&btInfinityMask)
+	#endif
+#endif//BT_USE_NEON
+
+#endif //BT_USE_SSE
 
 #ifdef BT_USE_NEON
 #include <arm_neon.h>
@@ -411,15 +434,15 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
 	
 #endif
 
-#define SIMD_2_PI         btScalar(6.283185307179586232)
-#define SIMD_PI           (SIMD_2_PI * btScalar(0.5))
-#define SIMD_HALF_PI      (SIMD_2_PI * btScalar(0.25))
+#define SIMD_PI           btScalar(3.1415926535897932384626433832795029)
+#define SIMD_2_PI         btScalar(2.0) * SIMD_PI
+#define SIMD_HALF_PI      (SIMD_PI * btScalar(0.5))
 #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
 #define SIMD_DEGS_PER_RAD  (btScalar(360.0) / SIMD_2_PI)
 #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
 
 #define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))		/* reciprocal square root */
-
+#define btRecip(x) (btScalar(1.0)/btScalar(x))
 
 #ifdef BT_USE_DOUBLE_PRECISION
 #define SIMD_EPSILON      DBL_EPSILON
@@ -610,6 +633,46 @@ SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src)
 	return d;
 }
 
+template<typename T>
+SIMD_FORCE_INLINE void btSetZero(T* a, int n)
+{
+  T* acurr = a;
+  size_t ncurr = n;
+  while (ncurr > 0) 
+  {
+    *(acurr++) = 0;
+    --ncurr;
+  }
+}
+
+
+SIMD_FORCE_INLINE btScalar btLargeDot(const btScalar *a, const btScalar *b, int n)
+{  
+  btScalar p0,q0,m0,p1,q1,m1,sum;
+  sum = 0;
+  n -= 2;
+  while (n >= 0) {
+    p0 = a[0]; q0 = b[0];
+    m0 = p0 * q0;
+    p1 = a[1]; q1 = b[1];
+    m1 = p1 * q1;
+    sum += m0;
+    sum += m1;
+    a += 2;
+    b += 2;
+    n -= 2;
+  }
+  n += 2;
+  while (n > 0) {
+    sum += (*a) * (*b);
+    a++;
+    b++;
+    n--;
+  }
+  return sum;
+}
+
+
 // returns normalized value in range [-SIMD_PI, SIMD_PI]
 SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) 
 {
@@ -628,6 +691,8 @@ SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
 	}
 }
 
+
+
 ///rudimentary class to provide type info
 struct btTypedObject
 {

Source/ThirdParty/Bullet/src/LinearMath/btSerializer.h

@@ -438,7 +438,7 @@ public:
 
 			buffer[9] = '2';
 			buffer[10] = '8';
-			buffer[11] = '1';
+			buffer[11] = '2';
 
 		}
 

Source/ThirdParty/Bullet/src/LinearMath/btVector3.cpp

@@ -51,7 +51,7 @@ long _maxdot_large( const float *vv, const float *vec, unsigned long count, floa
 long _maxdot_large( const float *vv, const float *vec, unsigned long count, float *dotResult )
 {
     const float4 *vertices = (const float4*) vv;
-    static const unsigned char indexTable[16] = {-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 };
+    static const unsigned char indexTable[16] = {(unsigned char)-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 };
     float4 dotMax = btAssign128( -BT_INFINITY,  -BT_INFINITY,  -BT_INFINITY,  -BT_INFINITY );
     float4 vvec = _mm_loadu_ps( vec );
     float4 vHi = btCastiTo128f(_mm_shuffle_epi32( btCastfTo128i( vvec), 0xaa ));          /// zzzz
@@ -436,7 +436,7 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa
 long _mindot_large( const float *vv, const float *vec, unsigned long count, float *dotResult )
 {
     const float4 *vertices = (const float4*) vv;
-    static const unsigned char indexTable[16] = {-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 };
+    static const unsigned char indexTable[16] = {(unsigned char)-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 };
     float4 dotmin = btAssign128( BT_INFINITY,  BT_INFINITY,  BT_INFINITY,  BT_INFINITY );
     float4 vvec = _mm_loadu_ps( vec );
     float4 vHi = btCastiTo128f(_mm_shuffle_epi32( btCastfTo128i( vvec), 0xaa ));          /// zzzz
@@ -823,7 +823,8 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa
 #elif defined BT_USE_NEON
 #define ARM_NEON_GCC_COMPATIBILITY  1
 #include <arm_neon.h>
-
+#include <sys/types.h>
+#include <sys/sysctl.h> //for sysctlbyname
 
 static long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult );
 static long _maxdot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult );
@@ -835,11 +836,34 @@ static long _mindot_large_sel( const float *vv, const float *vec, unsigned long
 long (*_maxdot_large)( const float *vv, const float *vec, unsigned long count, float *dotResult ) = _maxdot_large_sel;
 long (*_mindot_large)( const float *vv, const float *vec, unsigned long count, float *dotResult ) = _mindot_large_sel;
 
-extern "C" {int  _get_cpu_capabilities( void );}
+
+static inline uint32_t btGetCpuCapabilities( void )
+{
+    static uint32_t capabilities = 0;
+    static bool testedCapabilities = false;
+
+    if( 0 == testedCapabilities)
+    {
+        uint32_t hasFeature = 0;
+        size_t featureSize = sizeof( hasFeature );
+        int err = sysctlbyname( "hw.optional.neon_hpfp", &hasFeature, &featureSize, NULL, 0 );
+
+        if( 0 == err && hasFeature)
+            capabilities |= 0x2000;
+
+		testedCapabilities = true;
+    }
+    
+    return capabilities;
+}
+
+
+
 
 static long _maxdot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult )
 {
-    if( _get_cpu_capabilities() & 0x2000 )
+
+    if( btGetCpuCapabilities() & 0x2000 )
         _maxdot_large = _maxdot_large_v1;
     else
         _maxdot_large = _maxdot_large_v0;
@@ -849,7 +873,8 @@ static long _maxdot_large_sel( const float *vv, const float *vec, unsigned long
 
 static long _mindot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult )
 {
-    if( _get_cpu_capabilities() & 0x2000 )
+
+    if( btGetCpuCapabilities() & 0x2000 )
         _mindot_large = _mindot_large_v1;
     else
         _mindot_large = _mindot_large_v0;
@@ -872,8 +897,8 @@ long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, f
     float32x2_t dotMaxHi = (float32x2_t) { -BT_INFINITY, -BT_INFINITY };
     uint32x2_t indexLo = (uint32x2_t) {0, 1};
     uint32x2_t indexHi = (uint32x2_t) {2, 3};
-    uint32x2_t iLo = (uint32x2_t) {-1, -1};
-    uint32x2_t iHi = (uint32x2_t) {-1, -1};
+    uint32x2_t iLo = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
+    uint32x2_t iHi = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
     const uint32x2_t four = (uint32x2_t) {4,4};
 
     for( ; i+8 <= count; i+= 8 )
@@ -1059,7 +1084,7 @@ long _maxdot_large_v1( const float *vv, const float *vec, unsigned long count, f
     float32x4_t vHi = vdupq_lane_f32(vget_high_f32(vvec), 0);
     const uint32x4_t four = (uint32x4_t){ 4, 4, 4, 4 };
     uint32x4_t local_index = (uint32x4_t) {0, 1, 2, 3};
-    uint32x4_t index = (uint32x4_t) { -1, -1, -1, -1 };
+    uint32x4_t index = (uint32x4_t) { static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1) };
     float32x4_t maxDot = (float32x4_t) { -BT_INFINITY, -BT_INFINITY, -BT_INFINITY, -BT_INFINITY };
     
     unsigned long i = 0;
@@ -1257,8 +1282,8 @@ long _mindot_large_v0( const float *vv, const float *vec, unsigned long count, f
     float32x2_t dotMinHi = (float32x2_t) { BT_INFINITY, BT_INFINITY };
     uint32x2_t indexLo = (uint32x2_t) {0, 1};
     uint32x2_t indexHi = (uint32x2_t) {2, 3};
-    uint32x2_t iLo = (uint32x2_t) {-1, -1};
-    uint32x2_t iHi = (uint32x2_t) {-1, -1};
+    uint32x2_t iLo = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
+    uint32x2_t iHi = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
     const uint32x2_t four = (uint32x2_t) {4,4};
     
     for( ; i+8 <= count; i+= 8 )
@@ -1442,7 +1467,7 @@ long _mindot_large_v1( const float *vv, const float *vec, unsigned long count, f
     float32x4_t vHi = vdupq_lane_f32(vget_high_f32(vvec), 0);
     const uint32x4_t four = (uint32x4_t){ 4, 4, 4, 4 };
     uint32x4_t local_index = (uint32x4_t) {0, 1, 2, 3};
-    uint32x4_t index = (uint32x4_t) { -1, -1, -1, -1 };
+    uint32x4_t index = (uint32x4_t) { static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1) };
     float32x4_t minDot = (float32x4_t) { BT_INFINITY, BT_INFINITY, BT_INFINITY, BT_INFINITY };
     
     unsigned long i = 0;

Source/ThirdParty/Bullet/src/LinearMath/btVector3.h

@@ -69,7 +69,8 @@ subject to the following restrictions:
 #ifdef BT_USE_NEON
 
 const float32x4_t ATTRIBUTE_ALIGNED16(btvMzeroMask) = (float32x4_t){-0.0f, -0.0f, -0.0f, -0.0f};
-const int32x4_t ATTRIBUTE_ALIGNED16(btvFFF0Mask) = (int32x4_t){0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x0};
+const int32x4_t ATTRIBUTE_ALIGNED16(btvFFF0Mask) = (int32x4_t){static_cast<int32_t>(0xFFFFFFFF),
+	static_cast<int32_t>(0xFFFFFFFF), static_cast<int32_t>(0xFFFFFFFF), 0x0};
 const int32x4_t ATTRIBUTE_ALIGNED16(btvAbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF};
 const int32x4_t ATTRIBUTE_ALIGNED16(btv3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x0};
 
@@ -264,6 +265,12 @@ public:
 		return btSqrt(length2());
 	}
 
+	/**@brief Return the norm (length) of the vector */
+	SIMD_FORCE_INLINE btScalar norm() const
+	{
+		return length();
+	}
+
   /**@brief Return the distance squared between the ends of this and another vector
    * This is symantically treating the vector like a point */
 	SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
@@ -289,6 +296,9 @@ public:
    * x^2 + y^2 + z^2 = 1 */
 	SIMD_FORCE_INLINE btVector3& normalize() 
 	{
+		
+		btAssert(length() != btScalar(0));
+
 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)		
         // dot product first
 		__m128 vd = _mm_mul_ps(mVec128, mVec128);
@@ -722,7 +732,7 @@ public:
         return btVector3(r);
         
 #elif defined(BT_USE_NEON)
-        static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 };
+        static const uint32x4_t xyzMask = (const uint32x4_t){ static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), 0 };
         float32x4_t a0 = vmulq_f32( v0.mVec128, this->mVec128);
         float32x4_t a1 = vmulq_f32( v1.mVec128, this->mVec128);
         float32x4_t a2 = vmulq_f32( v2.mVec128, this->mVec128);
@@ -940,13 +950,9 @@ SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
 
 SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
 {
-#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
 	btVector3 norm = *this;
 
 	return norm.normalize();
-#else
-	return *this / length();
-#endif
 } 
 
 SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const

Source/ThirdParty/Bullet/src/btBulletDynamicsCommon.h

@@ -33,6 +33,8 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btUniversalConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btHinge2Constraint.h"
 #include "BulletDynamics/ConstraintSolver/btGearConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btFixedConstraint.h"
+
 
 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"