Updating Bullet Physics to https://github.com/bulletphysics/bullet3/commit/98ac437649ab3c2c61389abb8a131304db811b77

Source/ThirdParty/Bullet/src/Bullet-C-Api.h

@@ -1,176 +0,0 @@
-/*
-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.
-*/
-
-/*
-	Draft high-level generic physics C-API. For low-level access, use the physics SDK native API's.
-	Work in progress, functionality will be added on demand.
-
-	If possible, use the richer Bullet C++ API, by including "btBulletDynamicsCommon.h"
-*/
-
-#ifndef BULLET_C_API_H
-#define BULLET_C_API_H
-
-#define PL_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
-
-#ifdef BT_USE_DOUBLE_PRECISION
-typedef double	plReal;
-#else
-typedef float	plReal;
-#endif
-
-typedef plReal	plVector3[3];
-typedef plReal	plQuaternion[4];
-
-#ifdef __cplusplus
-extern "C" { 
-#endif
-
-/**	Particular physics SDK (C-API) */
-	PL_DECLARE_HANDLE(plPhysicsSdkHandle);
-
-/** 	Dynamics world, belonging to some physics SDK (C-API)*/
-	PL_DECLARE_HANDLE(plDynamicsWorldHandle);
-
-/** Rigid Body that can be part of a Dynamics World (C-API)*/	
-	PL_DECLARE_HANDLE(plRigidBodyHandle);
-
-/** 	Collision Shape/Geometry, property of a Rigid Body (C-API)*/
-	PL_DECLARE_HANDLE(plCollisionShapeHandle);
-
-/** Constraint for Rigid Bodies (C-API)*/
-	PL_DECLARE_HANDLE(plConstraintHandle);
-
-/** Triangle Mesh interface (C-API)*/
-	PL_DECLARE_HANDLE(plMeshInterfaceHandle);
-
-/** Broadphase Scene/Proxy Handles (C-API)*/
-	PL_DECLARE_HANDLE(plCollisionBroadphaseHandle);
-	PL_DECLARE_HANDLE(plBroadphaseProxyHandle);
-	PL_DECLARE_HANDLE(plCollisionWorldHandle);
-
-/**
-	Create and Delete a Physics SDK	
-*/
-
-	extern	plPhysicsSdkHandle	plNewBulletSdk(void); //this could be also another sdk, like ODE, PhysX etc.
-	extern	void		plDeletePhysicsSdk(plPhysicsSdkHandle	physicsSdk);
-
-/** Collision World, not strictly necessary, you can also just create a Dynamics World with Rigid Bodies which internally manages the Collision World with Collision Objects */
-
-	typedef void(*btBroadphaseCallback)(void* clientData, void* object1,void* object2);
-
-	extern plCollisionBroadphaseHandle	plCreateSapBroadphase(btBroadphaseCallback beginCallback,btBroadphaseCallback endCallback);
-
-	extern void	plDestroyBroadphase(plCollisionBroadphaseHandle bp);
-
-	extern 	plBroadphaseProxyHandle plCreateProxy(plCollisionBroadphaseHandle bp, void* clientData, plReal minX,plReal minY,plReal minZ, plReal maxX,plReal maxY, plReal maxZ);
-
-	extern void plDestroyProxy(plCollisionBroadphaseHandle bp, plBroadphaseProxyHandle proxyHandle);
-
-	extern void plSetBoundingBox(plBroadphaseProxyHandle proxyHandle, plReal minX,plReal minY,plReal minZ, plReal maxX,plReal maxY, plReal maxZ);
-
-/* todo: add pair cache support with queries like add/remove/find pair */
-	
-	extern plCollisionWorldHandle plCreateCollisionWorld(plPhysicsSdkHandle physicsSdk);
-
-/* todo: add/remove objects */
-	
-
-/* Dynamics World */
-
-	extern  plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdk);
-
-	extern  void           plDeleteDynamicsWorld(plDynamicsWorldHandle world);
-
-	extern	void	plStepSimulation(plDynamicsWorldHandle,	plReal	timeStep);
-
-	extern  void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object);
-
-	extern  void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object);
-
-
-/* Rigid Body  */
-
-	extern  plRigidBodyHandle plCreateRigidBody(	void* user_data,  float mass, plCollisionShapeHandle cshape );
-
-	extern  void plDeleteRigidBody(plRigidBodyHandle body);
-
-
-/* Collision Shape definition */
-
-	extern  plCollisionShapeHandle plNewSphereShape(plReal radius);
-	extern  plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z);
-	extern  plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height);	
-	extern  plCollisionShapeHandle plNewConeShape(plReal radius, plReal height);
-	extern  plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height);
-	extern	plCollisionShapeHandle plNewCompoundShape(void);
-	extern	void	plAddChildShape(plCollisionShapeHandle compoundShape,plCollisionShapeHandle childShape, plVector3 childPos,plQuaternion childOrn);
-
-	extern  void plDeleteShape(plCollisionShapeHandle shape);
-
-	/* Convex Meshes */
-	extern  plCollisionShapeHandle plNewConvexHullShape(void);
-	extern  void		plAddVertex(plCollisionShapeHandle convexHull, plReal x,plReal y,plReal z);
-/* Concave static triangle meshes */
-	extern  plMeshInterfaceHandle		   plNewMeshInterface(void);
-	extern  void		plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
-	extern  plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
-
-	extern  void plSetScaling(plCollisionShapeHandle shape, plVector3 scaling);
-
-/* SOLID has Response Callback/Table/Management */
-/* PhysX has Triggers, User Callbacks and filtering */
-/* ODE has the typedef void dNearCallback (void *data, dGeomID o1, dGeomID o2); */
-
-/*	typedef void plUpdatedPositionCallback(void* userData, plRigidBodyHandle	rbHandle, plVector3 pos); */
-/*	typedef void plUpdatedOrientationCallback(void* userData, plRigidBodyHandle	rbHandle, plQuaternion orientation); */
-
-	/* get world transform */
-	extern void	plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix);
-	extern void	plGetPosition(plRigidBodyHandle object,plVector3 position);
-	extern void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation);
-
-	/* set world transform (position/orientation) */
-	extern  void plSetPosition(plRigidBodyHandle object, const plVector3 position);
-	extern  void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation);
-	extern	void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient);
-	extern	void plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix);
-
-	typedef struct plRayCastResult {
-		plRigidBodyHandle		m_body;  
-		plCollisionShapeHandle	m_shape; 		
-		plVector3				m_positionWorld; 		
-		plVector3				m_normalWorld;
-	} plRayCastResult;
-
-	extern  int plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plRayCastResult res);
-
-	/* Sweep API */
-
-	/* extern  plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal); */
-
-	/* Continuous Collision Detection API */
-	
-	// needed for source/blender/blenkernel/intern/collision.c
-	double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3]);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif //BULLET_C_API_H
-

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

@@ -38,8 +38,9 @@ static DBVT_INLINE btDbvtVolume	merge(	const btDbvtVolume& a,
 									  const btDbvtVolume& b)
 {
 #if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)
-	ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]);
-	btDbvtVolume&	res=*(btDbvtVolume*)locals;
+	ATTRIBUTE_ALIGNED16( char locals[sizeof(btDbvtAabbMm)]);
+	btDbvtVolume* ptr = (btDbvtVolume*) locals;
+	btDbvtVolume&	res=*ptr;
 #else
 		btDbvtVolume	res;
 #endif
@@ -250,7 +251,8 @@ static btDbvtVolume				bounds(	const tNodeArray& leaves)
 {
 #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
 	ATTRIBUTE_ALIGNED16(char	locals[sizeof(btDbvtVolume)]);
-	btDbvtVolume&	volume=*(btDbvtVolume*)locals;
+	btDbvtVolume* ptr = (btDbvtVolume*) locals;
+	btDbvtVolume&	volume=*ptr;
 	volume=leaves[0]->volume;
 #else
 	btDbvtVolume volume=leaves[0]->volume;

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

@@ -189,7 +189,7 @@ bool	btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const
 
 	if ((!body0->isActive()) && (!body1->isActive()))
 		needsCollision = false;
-	else if (!body0->checkCollideWith(body1))
+	else if ((!body0->checkCollideWith(body1)) || (!body1->checkCollideWith(body0)))
 		needsCollision = false;
 	
 	return needsCollision ;

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

@@ -4,8 +4,8 @@ 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, 
+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.
@@ -31,10 +31,11 @@ btCollisionObject::btCollisionObject()
 		m_activationState1(1),
 		m_deactivationTime(btScalar(0.)),
 		m_friction(btScalar(0.5)),
-		m_rollingFriction(0.0f),
 		m_restitution(btScalar(0.)),
+		m_rollingFriction(0.0f),
 		m_internalType(CO_COLLISION_OBJECT),
 		m_userObjectPointer(0),
+		m_userIndex(-1),
 		m_hitFraction(btScalar(1.)),
 		m_ccdSweptSphereRadius(btScalar(0.)),
 		m_ccdMotionThreshold(btScalar(0.)),

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

@@ -92,11 +92,10 @@ protected:
 	int				m_internalType;
 
 	///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
-	union
-	{
-		void*			m_userObjectPointer;
-		int	m_userIndex;
-	};
+
+    void*			m_userObjectPointer;
+    
+    int	m_userIndex;
 
 	///time of impact calculation
 	btScalar		m_hitFraction; 
@@ -110,13 +109,11 @@ protected:
 	/// If some object should have elaborate collision filtering by sub-classes
 	int			m_checkCollideWith;
 
+	btAlignedObjectArray<const btCollisionObject*> m_objectsWithoutCollisionCheck;
+
 	///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation.
 	int			m_updateRevision;
 
-	virtual bool	checkCollideWithOverride(const btCollisionObject* /* co */) const
-	{
-		return true;
-	}
 
 public:
 
@@ -225,7 +222,34 @@ public:
 		return m_collisionShape;
 	}
 
-	
+	void	setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck)
+	{
+		if (ignoreCollisionCheck)
+		{
+			//We don't check for duplicates. Is it ok to leave that up to the user of this API?
+			//int index = m_objectsWithoutCollisionCheck.findLinearSearch(co);
+			//if (index == m_objectsWithoutCollisionCheck.size())
+			//{
+			m_objectsWithoutCollisionCheck.push_back(co);
+			//}
+		}
+		else
+		{
+			m_objectsWithoutCollisionCheck.remove(co);
+		}
+		m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0;
+	}
+
+	virtual bool	checkCollideWithOverride(const btCollisionObject*  co) const
+	{
+		int index = m_objectsWithoutCollisionCheck.findLinearSearch(co);
+		if (index < m_objectsWithoutCollisionCheck.size())
+		{
+			return false;
+		}
+		return true;
+	}
+
 
 	
 

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

@@ -1,43 +1,43 @@
-#ifndef BT_COLLISION_OBJECT_WRAPPER_H
-#define BT_COLLISION_OBJECT_WRAPPER_H
-
-///btCollisionObjectWrapperis an internal data structure. 
-///Most users can ignore this and use btCollisionObject and btCollisionShape instead
-class btCollisionShape;
-class btCollisionObject;
-class btTransform;
-#include "../../LinearMath/btScalar.h" // for SIMD_FORCE_INLINE definition
-
-#define BT_DECLARE_STACK_ONLY_OBJECT \
-	private: \
-		void* operator new(size_t size); \
-		void operator delete(void*);
-
-struct btCollisionObjectWrapper;
-struct btCollisionObjectWrapper
-{
-BT_DECLARE_STACK_ONLY_OBJECT
-
-private:
-	btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed.
-	btCollisionObjectWrapper* operator=(const btCollisionObjectWrapper&);
-
-public:
-	const btCollisionObjectWrapper* m_parent;
-	const btCollisionShape* m_shape;
-	const btCollisionObject* m_collisionObject;
-	const btTransform& m_worldTransform;
-	int		m_partId;
-	int		m_index;
-
-	btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, int partId, int index)
-	: m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform),
-	m_partId(partId), m_index(index)
-	{}
-
-	SIMD_FORCE_INLINE const btTransform& getWorldTransform() const { return m_worldTransform; }
-	SIMD_FORCE_INLINE const btCollisionObject* getCollisionObject() const { return m_collisionObject; }
-	SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_shape; }
-};
-
-#endif //BT_COLLISION_OBJECT_WRAPPER_H
+#ifndef BT_COLLISION_OBJECT_WRAPPER_H
+#define BT_COLLISION_OBJECT_WRAPPER_H
+
+///btCollisionObjectWrapperis an internal data structure. 
+///Most users can ignore this and use btCollisionObject and btCollisionShape instead
+class btCollisionShape;
+class btCollisionObject;
+class btTransform;
+#include "../../LinearMath/btScalar.h" // for SIMD_FORCE_INLINE definition
+
+#define BT_DECLARE_STACK_ONLY_OBJECT \
+	private: \
+		void* operator new(size_t size); \
+		void operator delete(void*);
+
+struct btCollisionObjectWrapper;
+struct btCollisionObjectWrapper
+{
+BT_DECLARE_STACK_ONLY_OBJECT
+
+private:
+	btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed.
+	btCollisionObjectWrapper* operator=(const btCollisionObjectWrapper&);
+
+public:
+	const btCollisionObjectWrapper* m_parent;
+	const btCollisionShape* m_shape;
+	const btCollisionObject* m_collisionObject;
+	const btTransform& m_worldTransform;
+	int		m_partId;
+	int		m_index;
+
+	btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, int partId, int index)
+	: m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform),
+	m_partId(partId), m_index(index)
+	{}
+
+	SIMD_FORCE_INLINE const btTransform& getWorldTransform() const { return m_worldTransform; }
+	SIMD_FORCE_INLINE const btCollisionObject* getCollisionObject() const { return m_collisionObject; }
+	SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_shape; }
+};
+
+#endif //BT_COLLISION_OBJECT_WRAPPER_H

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

@@ -13,8 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-// Modified by Lasse Oorni for Urho3D
-
 #include "btCollisionWorld.h"
 #include "btCollisionDispatcher.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
@@ -36,7 +34,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
 
 
@@ -294,12 +292,13 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 		btGjkConvexCast	gjkConvexCaster(castShape,convexShape,&simplexSolver);
 		
 		//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
-		bool condition = true;
+
 		btConvexCast* convexCasterPtr = 0;
-		if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest)
-			convexCasterPtr = &subSimplexConvexCaster;
-		else
+		//use kF_UseSubSimplexConvexCastRaytest by default
+		if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest)
 			convexCasterPtr = &gjkConvexCaster;
+		else
+			convexCasterPtr = &subSimplexConvexCaster;
 		
 		btConvexCast& convexCaster = *convexCasterPtr;
 
@@ -389,14 +388,7 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
 				triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
 			}
-			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
+			else
 			{
 				//generic (slower) case
 				btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
@@ -772,7 +764,7 @@ void	btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
 									hitPointLocal,
 									hitFraction);
 
-								bool	normalInWorldSpace = false;
+								bool	normalInWorldSpace = true;
 
 								return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
 							}
@@ -1234,12 +1226,10 @@ public:
 		  wv0 = m_worldTrans*triangle[0];
 		  wv1 = m_worldTrans*triangle[1];
 		  wv2 = m_worldTrans*triangle[2];
+		  btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
           
           if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals )
           {
-		    // Urho3D: calculate center only if needed
-		    btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
-		    
 		    btVector3 normal = (wv1-wv0).cross(wv2-wv0);
 		    normal.normalize();
 		    btVector3 normalColor(1,1,0);
@@ -1254,18 +1244,11 @@ public:
 
 void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
 {
-	// Urho3D: perform AABB visibility test first
-	btVector3 aabbMin, aabbMax;
-	shape->getAabb(worldTransform, aabbMin, aabbMax);
-	if (!getDebugDrawer()->isVisible(aabbMin, aabbMax))
-		return;
-	
 	// Draw a small simplex at the center of the object
-	getDebugDrawer()->drawTransform(worldTransform,1);
-	
-	// Urho3D: never draw heightfields as they are potentially huge
-	if (shape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
-		return;
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames)
+	{
+		getDebugDrawer()->drawTransform(worldTransform,1);
+	}
 
 	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
 	{
@@ -1445,19 +1428,22 @@ void	btCollisionWorld::debugDrawWorld()
 {
 	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
 	{
-		int numManifolds = getDispatcher()->getNumManifolds();
-		btVector3 color(1,1,0);
-		for (int i=0;i<numManifolds;i++)
+		if (getDispatcher())
 		{
-			btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
-			//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
-			//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
-
-			int numContacts = contactManifold->getNumContacts();
-			for (int j=0;j<numContacts;j++)
+			int numManifolds = getDispatcher()->getNumManifolds();
+			btVector3 color(1,1,0);
+			for (int i=0;i<numManifolds;i++)
 			{
-				btManifoldPoint& cp = contactManifold->getContactPoint(j);
-				getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
+				btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
+				//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
+				//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+
+				int numContacts = contactManifold->getNumContacts();
+				for (int j=0;j<numContacts;j++)
+				{
+					btManifoldPoint& cp = contactManifold->getContactPoint(j);
+					getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
+				}
 			}
 		}
 	}
@@ -1526,15 +1512,6 @@ void	btCollisionWorld::debugDrawWorld()
 void	btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
 {
 	int i;
-	//serialize all collision objects
-	for (i=0;i<m_collisionObjects.size();i++)
-	{
-		btCollisionObject* colObj = m_collisionObjects[i];
-		if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
-		{
-			colObj->serializeSingleObject(serializer);
-		}
-	}
 
 	///keep track of shapes already serialized
 	btHashMap<btHashPtr,btCollisionShape*>	serializedShapes;
@@ -1551,6 +1528,15 @@ void	btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
 		}
 	}
 
+	//serialize all collision objects
+	for (i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
+		{
+			colObj->serializeSingleObject(serializer);
+		}
+	}
 }
 
 

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

@@ -0,0 +1,1147 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2014 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 "btCollisionWorldImporter.h"
+#include "btBulletCollisionCommon.h"
+#include "LinearMath/btSerializer.h" //for btBulletSerializedArrays definition
+
+#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT
+#include "BulletCollision/Gimpact/btGImpactShape.h"
+#endif //SUPPORT_GIMPACT_SHAPE_IMPORT
+
+btCollisionWorldImporter::btCollisionWorldImporter(btCollisionWorld* world)
+:m_collisionWorld(world),
+m_verboseMode(0)
+{
+
+}
+
+btCollisionWorldImporter::~btCollisionWorldImporter()
+{
+}
+
+
+
+
+
+bool	btCollisionWorldImporter::convertAllObjects( btBulletSerializedArrays* arrays)
+{
+
+	m_shapeMap.clear();
+	m_bodyMap.clear();
+
+	int i;
+
+	for (i=0;i<arrays->m_bvhsDouble.size();i++)
+	{
+		btOptimizedBvh* bvh = createOptimizedBvh();
+		btQuantizedBvhDoubleData* bvhData = arrays->m_bvhsDouble[i];
+		bvh->deSerializeDouble(*bvhData);
+		m_bvhMap.insert(arrays->m_bvhsDouble[i],bvh);
+	}
+	for (i=0;i<arrays->m_bvhsFloat.size();i++)
+    {
+        btOptimizedBvh* bvh = createOptimizedBvh();
+   		btQuantizedBvhFloatData* bvhData = arrays->m_bvhsFloat[i];
+		bvh->deSerializeFloat(*bvhData);
+		m_bvhMap.insert(arrays->m_bvhsFloat[i],bvh);
+	}
+
+
+
+
+
+	for (i=0;i<arrays->m_colShapeData.size();i++)
+	{
+		btCollisionShapeData* shapeData = arrays->m_colShapeData[i];
+		btCollisionShape* shape = convertCollisionShape(shapeData);
+		if (shape)
+		{
+	//		printf("shapeMap.insert(%x,%x)\n",shapeData,shape);
+			m_shapeMap.insert(shapeData,shape);
+		}
+
+		if (shape&& shapeData->m_name)
+		{
+			char* newname = duplicateName(shapeData->m_name);
+			m_objectNameMap.insert(shape,newname);
+			m_nameShapeMap.insert(newname,shape);
+		}
+	}
+
+
+	for (i=0;i<arrays->m_collisionObjectDataDouble.size();i++)
+	{
+        btCollisionObjectDoubleData* colObjData = arrays->m_collisionObjectDataDouble[i];
+        btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape);
+        if (shapePtr && *shapePtr)
+        {
+            btTransform startTransform;
+            colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f;
+            startTransform.deSerializeDouble(colObjData->m_worldTransform);
+
+            btCollisionShape* shape = (btCollisionShape*)*shapePtr;
+            btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name);
+            body->setFriction(btScalar(colObjData->m_friction));
+            body->setRestitution(btScalar(colObjData->m_restitution));
+
+#ifdef USE_INTERNAL_EDGE_UTILITY
+            if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+            {
+                btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape;
+                if (trimesh->getTriangleInfoMap())
+                {
+                    body->setCollisionFlags(body->getCollisionFlags()  | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+                }
+            }
+#endif //USE_INTERNAL_EDGE_UTILITY
+            m_bodyMap.insert(colObjData,body);
+        } else
+        {
+            printf("error: no shape found\n");
+        }
+	}
+	for (i=0;i<arrays->m_collisionObjectDataFloat.size();i++)
+	{
+        btCollisionObjectFloatData* colObjData = arrays->m_collisionObjectDataFloat[i];
+        btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape);
+        if (shapePtr && *shapePtr)
+        {
+            btTransform startTransform;
+            colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f;
+            startTransform.deSerializeFloat(colObjData->m_worldTransform);
+
+            btCollisionShape* shape = (btCollisionShape*)*shapePtr;
+            btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name);
+
+#ifdef USE_INTERNAL_EDGE_UTILITY
+            if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+            {
+                btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape;
+                if (trimesh->getTriangleInfoMap())
+                {
+                    body->setCollisionFlags(body->getCollisionFlags()  | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+                }
+            }
+#endif //USE_INTERNAL_EDGE_UTILITY
+            m_bodyMap.insert(colObjData,body);
+        } else
+        {
+            printf("error: no shape found\n");
+        }
+    }
+
+	return true;
+}
+
+
+
+void btCollisionWorldImporter::deleteAllData()
+{
+	int i;
+
+	for (i=0;i<m_allocatedCollisionObjects.size();i++)
+	{
+		if(m_collisionWorld)
+			m_collisionWorld->removeCollisionObject(m_allocatedCollisionObjects[i]);
+		delete m_allocatedCollisionObjects[i];
+	}
+
+	m_allocatedCollisionObjects.clear();
+
+
+	for (i=0;i<m_allocatedCollisionShapes.size();i++)
+	{
+		delete m_allocatedCollisionShapes[i];
+	}
+	m_allocatedCollisionShapes.clear();
+
+
+	for (i=0;i<m_allocatedBvhs.size();i++)
+	{
+		delete m_allocatedBvhs[i];
+	}
+	m_allocatedBvhs.clear();
+
+	for (i=0;i<m_allocatedTriangleInfoMaps.size();i++)
+	{
+		delete m_allocatedTriangleInfoMaps[i];
+	}
+	m_allocatedTriangleInfoMaps.clear();
+	for (i=0;i<m_allocatedTriangleIndexArrays.size();i++)
+	{
+		delete m_allocatedTriangleIndexArrays[i];
+	}
+	m_allocatedTriangleIndexArrays.clear();
+	for (i=0;i<m_allocatedNames.size();i++)
+	{
+		delete[] m_allocatedNames[i];
+	}
+	m_allocatedNames.clear();
+
+	for (i=0;i<m_allocatedbtStridingMeshInterfaceDatas.size();i++)
+	{
+		btStridingMeshInterfaceData* curData = m_allocatedbtStridingMeshInterfaceDatas[i];
+
+		for(int a = 0;a < curData->m_numMeshParts;a++)
+		{
+			btMeshPartData* curPart = &curData->m_meshPartsPtr[a];
+			if(curPart->m_vertices3f)
+				delete [] curPart->m_vertices3f;
+
+			if(curPart->m_vertices3d)
+				delete [] curPart->m_vertices3d;
+
+			if(curPart->m_indices32)
+				delete [] curPart->m_indices32;
+
+			if(curPart->m_3indices16)
+				delete [] curPart->m_3indices16;
+
+			if(curPart->m_indices16)
+				delete [] curPart->m_indices16;
+
+			if (curPart->m_3indices8)
+				delete [] curPart->m_3indices8;
+
+		}
+		delete [] curData->m_meshPartsPtr;
+		delete curData;
+	}
+	m_allocatedbtStridingMeshInterfaceDatas.clear();
+
+	for (i=0;i<m_indexArrays.size();i++)
+	{
+		btAlignedFree(m_indexArrays[i]);
+	}
+  m_indexArrays.clear();
+
+	for (i=0;i<m_shortIndexArrays.size();i++)
+	{
+		btAlignedFree(m_shortIndexArrays[i]);
+	}
+  m_shortIndexArrays.clear();
+
+	for (i=0;i<m_charIndexArrays.size();i++)
+	{
+		btAlignedFree(m_charIndexArrays[i]);
+	}
+  m_charIndexArrays.clear();
+
+	for (i=0;i<m_floatVertexArrays.size();i++)
+	{
+		btAlignedFree(m_floatVertexArrays[i]);
+	}
+  m_floatVertexArrays.clear();
+
+	for (i=0;i<m_doubleVertexArrays.size();i++)
+	{
+		btAlignedFree(m_doubleVertexArrays[i]);
+	}
+   m_doubleVertexArrays.clear();
+
+
+}
+
+
+
+btCollisionShape* btCollisionWorldImporter::convertCollisionShape(  btCollisionShapeData* shapeData  )
+{
+	btCollisionShape* shape = 0;
+
+	switch (shapeData->m_shapeType)
+		{
+	case STATIC_PLANE_PROXYTYPE:
+		{
+			btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)shapeData;
+			btVector3 planeNormal,localScaling;
+			planeNormal.deSerializeFloat(planeData->m_planeNormal);
+			localScaling.deSerializeFloat(planeData->m_localScaling);
+			shape = createPlaneShape(planeNormal,planeData->m_planeConstant);
+			shape->setLocalScaling(localScaling);
+
+			break;
+		}
+	case SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE:
+		{
+			btScaledTriangleMeshShapeData* scaledMesh = (btScaledTriangleMeshShapeData*) shapeData;
+			btCollisionShapeData* colShapeData = (btCollisionShapeData*) &scaledMesh->m_trimeshShapeData;
+			colShapeData->m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
+			btCollisionShape* childShape = convertCollisionShape(colShapeData);
+			btBvhTriangleMeshShape* meshShape = (btBvhTriangleMeshShape*)childShape;
+			btVector3 localScaling;
+			localScaling.deSerializeFloat(scaledMesh->m_localScaling);
+
+			shape = createScaledTrangleMeshShape(meshShape, localScaling);
+			break;
+		}
+#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT
+	case GIMPACT_SHAPE_PROXYTYPE:
+		{
+			btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*) shapeData;
+			if (gimpactData->m_gimpactSubType == CONST_GIMPACT_TRIMESH_SHAPE)
+			{
+				btStridingMeshInterfaceData* interfaceData = createStridingMeshInterfaceData(&gimpactData->m_meshInterface);
+				btTriangleIndexVertexArray* meshInterface = createMeshInterface(*interfaceData);
+
+
+				btGImpactMeshShape* gimpactShape = createGimpactShape(meshInterface);
+				btVector3 localScaling;
+				localScaling.deSerializeFloat(gimpactData->m_localScaling);
+				gimpactShape->setLocalScaling(localScaling);
+				gimpactShape->setMargin(btScalar(gimpactData->m_collisionMargin));
+				gimpactShape->updateBound();
+				shape = gimpactShape;
+			} else
+			{
+				printf("unsupported gimpact sub type\n");
+			}
+			break;
+		}
+#endif //SUPPORT_GIMPACT_SHAPE_IMPORT
+	//The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API
+	//so deal with this
+		case CAPSULE_SHAPE_PROXYTYPE:
+		{
+			btCapsuleShapeData* capData = (btCapsuleShapeData*)shapeData;
+
+
+			switch (capData->m_upAxis)
+			{
+			case 0:
+				{
+					shape = createCapsuleShapeX(1,1);
+					break;
+				}
+			case 1:
+				{
+					shape = createCapsuleShapeY(1,1);
+					break;
+				}
+			case 2:
+				{
+					shape = createCapsuleShapeZ(1,1);
+					break;
+				}
+			default:
+				{
+					printf("error: wrong up axis for btCapsuleShape\n");
+				}
+
+
+			};
+			if (shape)
+			{
+				btCapsuleShape* cap = (btCapsuleShape*) shape;
+				cap->deSerializeFloat(capData);
+			}
+			break;
+		}
+		case CYLINDER_SHAPE_PROXYTYPE:
+		case CONE_SHAPE_PROXYTYPE:
+		case BOX_SHAPE_PROXYTYPE:
+		case SPHERE_SHAPE_PROXYTYPE:
+		case MULTI_SPHERE_SHAPE_PROXYTYPE:
+		case CONVEX_HULL_SHAPE_PROXYTYPE:
+			{
+				btConvexInternalShapeData* bsd = (btConvexInternalShapeData*)shapeData;
+				btVector3 implicitShapeDimensions;
+				implicitShapeDimensions.deSerializeFloat(bsd->m_implicitShapeDimensions);
+				btVector3 localScaling;
+				localScaling.deSerializeFloat(bsd->m_localScaling);
+				btVector3 margin(bsd->m_collisionMargin,bsd->m_collisionMargin,bsd->m_collisionMargin);
+				switch (shapeData->m_shapeType)
+				{
+					case BOX_SHAPE_PROXYTYPE:
+						{
+							btBoxShape* box= (btBoxShape*)createBoxShape(implicitShapeDimensions/localScaling+margin);
+							//box->initializePolyhedralFeatures();
+							shape = box;
+
+							break;
+						}
+					case SPHERE_SHAPE_PROXYTYPE:
+						{
+							shape = createSphereShape(implicitShapeDimensions.getX());
+							break;
+						}
+
+					case CYLINDER_SHAPE_PROXYTYPE:
+						{
+							btCylinderShapeData* cylData = (btCylinderShapeData*) shapeData;
+							btVector3 halfExtents = implicitShapeDimensions+margin;
+							switch (cylData->m_upAxis)
+							{
+							case 0:
+								{
+									shape = createCylinderShapeX(halfExtents.getY(),halfExtents.getX());
+									break;
+								}
+							case 1:
+								{
+									shape = createCylinderShapeY(halfExtents.getX(),halfExtents.getY());
+									break;
+								}
+							case 2:
+								{
+									shape = createCylinderShapeZ(halfExtents.getX(),halfExtents.getZ());
+									break;
+								}
+							default:
+								{
+									printf("unknown Cylinder up axis\n");
+								}
+
+							};
+
+
+
+							break;
+						}
+					case CONE_SHAPE_PROXYTYPE:
+						{
+							btConeShapeData* conData = (btConeShapeData*) shapeData;
+							btVector3 halfExtents = implicitShapeDimensions;//+margin;
+							switch (conData->m_upIndex)
+							{
+							case 0:
+								{
+									shape = createConeShapeX(halfExtents.getY(),halfExtents.getX());
+									break;
+								}
+							case 1:
+								{
+									shape = createConeShapeY(halfExtents.getX(),halfExtents.getY());
+									break;
+								}
+							case 2:
+								{
+									shape = createConeShapeZ(halfExtents.getX(),halfExtents.getZ());
+									break;
+								}
+							default:
+								{
+									printf("unknown Cone up axis\n");
+								}
+
+							};
+
+
+
+							break;
+						}
+					case MULTI_SPHERE_SHAPE_PROXYTYPE:
+						{
+							btMultiSphereShapeData* mss = (btMultiSphereShapeData*)bsd;
+							int numSpheres = mss->m_localPositionArraySize;
+
+							btAlignedObjectArray<btVector3> tmpPos;
+							btAlignedObjectArray<btScalar> radii;
+							radii.resize(numSpheres);
+							tmpPos.resize(numSpheres);
+							int i;
+							for ( i=0;i<numSpheres;i++)
+							{
+								tmpPos[i].deSerializeFloat(mss->m_localPositionArrayPtr[i].m_pos);
+								radii[i] = mss->m_localPositionArrayPtr[i].m_radius;
+							}
+							shape = createMultiSphereShape(&tmpPos[0],&radii[0],numSpheres);
+							break;
+						}
+					case CONVEX_HULL_SHAPE_PROXYTYPE:
+						{
+						//	int sz = sizeof(btConvexHullShapeData);
+						//	int sz2 = sizeof(btConvexInternalShapeData);
+						//	int sz3 = sizeof(btCollisionShapeData);
+							btConvexHullShapeData* convexData = (btConvexHullShapeData*)bsd;
+							int numPoints = convexData->m_numUnscaledPoints;
+
+							btAlignedObjectArray<btVector3> tmpPoints;
+							tmpPoints.resize(numPoints);
+							int i;
+							for ( i=0;i<numPoints;i++)
+							{
+#ifdef BT_USE_DOUBLE_PRECISION
+							if (convexData->m_unscaledPointsDoublePtr)
+								tmpPoints[i].deSerialize(convexData->m_unscaledPointsDoublePtr[i]);
+							if (convexData->m_unscaledPointsFloatPtr)
+								tmpPoints[i].deSerializeFloat(convexData->m_unscaledPointsFloatPtr[i]);
+#else
+							if (convexData->m_unscaledPointsFloatPtr)
+								tmpPoints[i].deSerialize(convexData->m_unscaledPointsFloatPtr[i]);
+							if (convexData->m_unscaledPointsDoublePtr)
+								tmpPoints[i].deSerializeDouble(convexData->m_unscaledPointsDoublePtr[i]);
+#endif //BT_USE_DOUBLE_PRECISION
+							}
+							btConvexHullShape* hullShape = createConvexHullShape();
+							for (i=0;i<numPoints;i++)
+							{
+								hullShape->addPoint(tmpPoints[i]);
+							}
+							hullShape->setMargin(bsd->m_collisionMargin);
+							//hullShape->initializePolyhedralFeatures();
+							shape = hullShape;
+							break;
+						}
+					default:
+						{
+							printf("error: cannot create shape type (%d)\n",shapeData->m_shapeType);
+						}
+				}
+
+				if (shape)
+				{
+					shape->setMargin(bsd->m_collisionMargin);
+
+					btVector3 localScaling;
+					localScaling.deSerializeFloat(bsd->m_localScaling);
+					shape->setLocalScaling(localScaling);
+
+				}
+				break;
+			}
+		case TRIANGLE_MESH_SHAPE_PROXYTYPE:
+		{
+			btTriangleMeshShapeData* trimesh = (btTriangleMeshShapeData*)shapeData;
+			btStridingMeshInterfaceData* interfaceData = createStridingMeshInterfaceData(&trimesh->m_meshInterface);
+			btTriangleIndexVertexArray* meshInterface = createMeshInterface(*interfaceData);
+			if (!meshInterface->getNumSubParts())
+			{
+				return 0;
+			}
+
+			btVector3 scaling; scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling);
+			meshInterface->setScaling(scaling);
+
+
+			btOptimizedBvh* bvh = 0;
+#if 1
+			if (trimesh->m_quantizedFloatBvh)
+			{
+				btOptimizedBvh** bvhPtr = m_bvhMap.find(trimesh->m_quantizedFloatBvh);
+				if (bvhPtr && *bvhPtr)
+				{
+					bvh = *bvhPtr;
+				} else
+				{
+					bvh = createOptimizedBvh();
+					bvh->deSerializeFloat(*trimesh->m_quantizedFloatBvh);
+				}
+			}
+			if (trimesh->m_quantizedDoubleBvh)
+			{
+				btOptimizedBvh** bvhPtr = m_bvhMap.find(trimesh->m_quantizedDoubleBvh);
+				if (bvhPtr && *bvhPtr)
+				{
+					bvh = *bvhPtr;
+				} else
+				{
+					bvh = createOptimizedBvh();
+					bvh->deSerializeDouble(*trimesh->m_quantizedDoubleBvh);
+				}
+			}
+#endif
+
+
+			btBvhTriangleMeshShape* trimeshShape = createBvhTriangleMeshShape(meshInterface,bvh);
+			trimeshShape->setMargin(trimesh->m_collisionMargin);
+			shape = trimeshShape;
+
+			if (trimesh->m_triangleInfoMap)
+			{
+				btTriangleInfoMap* map = createTriangleInfoMap();
+				map->deSerialize(*trimesh->m_triangleInfoMap);
+				trimeshShape->setTriangleInfoMap(map);
+
+#ifdef USE_INTERNAL_EDGE_UTILITY
+				gContactAddedCallback = btAdjustInternalEdgeContactsCallback;
+#endif //USE_INTERNAL_EDGE_UTILITY
+
+			}
+
+			//printf("trimesh->m_collisionMargin=%f\n",trimesh->m_collisionMargin);
+			break;
+		}
+		case COMPOUND_SHAPE_PROXYTYPE:
+			{
+				btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData;
+				btCompoundShape* compoundShape = createCompoundShape();
+
+				btCompoundShapeChildData* childShapeDataArray = &compoundData->m_childShapePtr[0];
+
+
+				btAlignedObjectArray<btCollisionShape*> childShapes;
+				for (int i=0;i<compoundData->m_numChildShapes;i++)
+				{
+					btCompoundShapeChildData* ptr = &compoundData->m_childShapePtr[i];
+
+					btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape;
+
+					btCollisionShape* childShape = convertCollisionShape(cd);
+					if (childShape)
+					{
+						btTransform localTransform;
+						localTransform.deSerializeFloat(compoundData->m_childShapePtr[i].m_transform);
+						compoundShape->addChildShape(localTransform,childShape);
+					} else
+					{
+#ifdef _DEBUG
+						printf("error: couldn't create childShape for compoundShape\n");
+#endif
+					}
+
+				}
+				shape = compoundShape;
+
+				break;
+			}
+		case SOFTBODY_SHAPE_PROXYTYPE:
+			{
+				return 0;
+			}
+		default:
+			{
+#ifdef _DEBUG
+				printf("unsupported shape type (%d)\n",shapeData->m_shapeType);
+#endif
+			}
+		}
+
+		return shape;
+
+}
+
+
+
+char* btCollisionWorldImporter::duplicateName(const char* name)
+{
+	if (name)
+	{
+		int l = (int)strlen(name);
+		char* newName = new char[l+1];
+		memcpy(newName,name,l);
+		newName[l] = 0;
+		m_allocatedNames.push_back(newName);
+		return newName;
+	}
+	return 0;
+}
+
+
+
+
+
+
+
+
+
+
+
+btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStridingMeshInterfaceData&  meshData)
+{
+	btTriangleIndexVertexArray* meshInterface = createTriangleMeshContainer();
+
+	for (int i=0;i<meshData.m_numMeshParts;i++)
+	{
+		btIndexedMesh meshPart;
+		meshPart.m_numTriangles = meshData.m_meshPartsPtr[i].m_numTriangles;
+		meshPart.m_numVertices = meshData.m_meshPartsPtr[i].m_numVertices;
+
+
+		if (meshData.m_meshPartsPtr[i].m_indices32)
+		{
+			meshPart.m_indexType = PHY_INTEGER;
+			meshPart.m_triangleIndexStride = 3*sizeof(int);
+			int* indexArray = (int*)btAlignedAlloc(sizeof(int)*3*meshPart.m_numTriangles,16);
+			m_indexArrays.push_back(indexArray);
+			for (int j=0;j<3*meshPart.m_numTriangles;j++)
+			{
+				indexArray[j] = meshData.m_meshPartsPtr[i].m_indices32[j].m_value;
+			}
+			meshPart.m_triangleIndexBase = (const unsigned char*)indexArray;
+		} else
+		{
+			if (meshData.m_meshPartsPtr[i].m_3indices16)
+			{
+				meshPart.m_indexType = PHY_SHORT;
+				meshPart.m_triangleIndexStride = sizeof(short int)*3;//sizeof(btShortIntIndexTripletData);
+
+				short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int)*3*meshPart.m_numTriangles,16);
+				m_shortIndexArrays.push_back(indexArray);
+
+				for (int j=0;j<meshPart.m_numTriangles;j++)
+				{
+					indexArray[3*j] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[0];
+					indexArray[3*j+1] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[1];
+					indexArray[3*j+2] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[2];
+				}
+
+				meshPart.m_triangleIndexBase = (const unsigned char*)indexArray;
+			}
+			if (meshData.m_meshPartsPtr[i].m_indices16)
+			{
+				meshPart.m_indexType = PHY_SHORT;
+				meshPart.m_triangleIndexStride = 3*sizeof(short int);
+				short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int)*3*meshPart.m_numTriangles,16);
+				m_shortIndexArrays.push_back(indexArray);
+				for (int j=0;j<3*meshPart.m_numTriangles;j++)
+				{
+					indexArray[j] = meshData.m_meshPartsPtr[i].m_indices16[j].m_value;
+				}
+
+				meshPart.m_triangleIndexBase = (const unsigned char*)indexArray;
+			}
+
+			if (meshData.m_meshPartsPtr[i].m_3indices8)
+			{
+				meshPart.m_indexType = PHY_UCHAR;
+				meshPart.m_triangleIndexStride = sizeof(unsigned char)*3;
+
+				unsigned char* indexArray = (unsigned char*)btAlignedAlloc(sizeof(unsigned char)*3*meshPart.m_numTriangles,16);
+				m_charIndexArrays.push_back(indexArray);
+
+				for (int j=0;j<meshPart.m_numTriangles;j++)
+				{
+					indexArray[3*j] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[0];
+					indexArray[3*j+1] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[1];
+					indexArray[3*j+2] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[2];
+				}
+
+				meshPart.m_triangleIndexBase = (const unsigned char*)indexArray;
+			}
+		}
+
+		if (meshData.m_meshPartsPtr[i].m_vertices3f)
+		{
+			meshPart.m_vertexType = PHY_FLOAT;
+			meshPart.m_vertexStride = sizeof(btVector3FloatData);
+			btVector3FloatData* vertices = (btVector3FloatData*) btAlignedAlloc(sizeof(btVector3FloatData)*meshPart.m_numVertices,16);
+			m_floatVertexArrays.push_back(vertices);
+
+			for (int j=0;j<meshPart.m_numVertices;j++)
+			{
+				vertices[j].m_floats[0] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[0];
+				vertices[j].m_floats[1] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[1];
+				vertices[j].m_floats[2] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[2];
+				vertices[j].m_floats[3] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[3];
+			}
+			meshPart.m_vertexBase = (const unsigned char*)vertices;
+		} else
+		{
+			meshPart.m_vertexType = PHY_DOUBLE;
+			meshPart.m_vertexStride = sizeof(btVector3DoubleData);
+
+
+			btVector3DoubleData* vertices = (btVector3DoubleData*) btAlignedAlloc(sizeof(btVector3DoubleData)*meshPart.m_numVertices,16);
+			m_doubleVertexArrays.push_back(vertices);
+
+			for (int j=0;j<meshPart.m_numVertices;j++)
+			{
+				vertices[j].m_floats[0] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[0];
+				vertices[j].m_floats[1] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[1];
+				vertices[j].m_floats[2] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[2];
+				vertices[j].m_floats[3] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[3];
+			}
+			meshPart.m_vertexBase = (const unsigned char*)vertices;
+		}
+
+		if (meshPart.m_triangleIndexBase && meshPart.m_vertexBase)
+		{
+			meshInterface->addIndexedMesh(meshPart,meshPart.m_indexType);
+		}
+	}
+
+	return meshInterface;
+}
+
+
+btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData)
+{
+	//create a new btStridingMeshInterfaceData that is an exact copy of shapedata and store it in the WorldImporter
+	btStridingMeshInterfaceData* newData = new btStridingMeshInterfaceData;
+
+	newData->m_scaling = interfaceData->m_scaling;
+	newData->m_numMeshParts = interfaceData->m_numMeshParts;
+	newData->m_meshPartsPtr = new btMeshPartData[newData->m_numMeshParts];
+
+	for(int i = 0;i < newData->m_numMeshParts;i++)
+	{
+		btMeshPartData* curPart = &interfaceData->m_meshPartsPtr[i];
+		btMeshPartData* curNewPart = &newData->m_meshPartsPtr[i];
+
+		curNewPart->m_numTriangles = curPart->m_numTriangles;
+		curNewPart->m_numVertices = curPart->m_numVertices;
+
+		if(curPart->m_vertices3f)
+		{
+			curNewPart->m_vertices3f = new btVector3FloatData[curNewPart->m_numVertices];
+			memcpy(curNewPart->m_vertices3f,curPart->m_vertices3f,sizeof(btVector3FloatData) * curNewPart->m_numVertices);
+		}
+		else
+			curNewPart->m_vertices3f = NULL;
+
+		if(curPart->m_vertices3d)
+		{
+			curNewPart->m_vertices3d = new btVector3DoubleData[curNewPart->m_numVertices];
+			memcpy(curNewPart->m_vertices3d,curPart->m_vertices3d,sizeof(btVector3DoubleData) * curNewPart->m_numVertices);
+		}
+		else
+			curNewPart->m_vertices3d = NULL;
+
+		int numIndices = curNewPart->m_numTriangles * 3;
+		///the m_3indices8 was not initialized in some Bullet versions, this can cause crashes at loading time
+		///we catch it by only dealing with m_3indices8 if none of the other indices are initialized
+		bool uninitialized3indices8Workaround =false;
+
+		if(curPart->m_indices32)
+		{
+			uninitialized3indices8Workaround=true;
+			curNewPart->m_indices32 = new btIntIndexData[numIndices];
+			memcpy(curNewPart->m_indices32,curPart->m_indices32,sizeof(btIntIndexData) * numIndices);
+		}
+		else
+			curNewPart->m_indices32 = NULL;
+
+		if(curPart->m_3indices16)
+		{
+			uninitialized3indices8Workaround=true;
+			curNewPart->m_3indices16 = new btShortIntIndexTripletData[curNewPart->m_numTriangles];
+			memcpy(curNewPart->m_3indices16,curPart->m_3indices16,sizeof(btShortIntIndexTripletData) * curNewPart->m_numTriangles);
+		}
+		else
+			curNewPart->m_3indices16 = NULL;
+
+		if(curPart->m_indices16)
+		{
+			uninitialized3indices8Workaround=true;
+			curNewPart->m_indices16 = new btShortIntIndexData[numIndices];
+			memcpy(curNewPart->m_indices16,curPart->m_indices16,sizeof(btShortIntIndexData) * numIndices);
+		}
+		else
+			curNewPart->m_indices16 = NULL;
+
+		if(!uninitialized3indices8Workaround && curPart->m_3indices8)
+		{
+			curNewPart->m_3indices8 = new btCharIndexTripletData[curNewPart->m_numTriangles];
+			memcpy(curNewPart->m_3indices8,curPart->m_3indices8,sizeof(btCharIndexTripletData) * curNewPart->m_numTriangles);
+		}
+		else
+			curNewPart->m_3indices8 = NULL;
+
+	}
+
+	m_allocatedbtStridingMeshInterfaceDatas.push_back(newData);
+
+	return(newData);
+}
+
+#ifdef USE_INTERNAL_EDGE_UTILITY
+extern ContactAddedCallback		gContactAddedCallback;
+
+static bool btAdjustInternalEdgeContactsCallback(btManifoldPoint& cp,	const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1)
+{
+
+	btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1);
+		//btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_BACKFACE_MODE);
+		//btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_DOUBLE_SIDED+BT_TRIANGLE_CONCAVE_DOUBLE_SIDED);
+	return true;
+}
+#endif //USE_INTERNAL_EDGE_UTILITY
+
+
+/*
+btRigidBody*  btWorldImporter::createRigidBody(bool isDynamic, btScalar mass, const btTransform& startTransform,btCollisionShape* shape,const char* bodyName)
+{
+	btVector3 localInertia;
+	localInertia.setZero();
+
+	if (mass)
+		shape->calculateLocalInertia(mass,localInertia);
+
+	btRigidBody* body = new btRigidBody(mass,0,shape,localInertia);
+	body->setWorldTransform(startTransform);
+
+	if (m_dynamicsWorld)
+		m_dynamicsWorld->addRigidBody(body);
+
+	if (bodyName)
+	{
+		char* newname = duplicateName(bodyName);
+		m_objectNameMap.insert(body,newname);
+		m_nameBodyMap.insert(newname,body);
+	}
+	m_allocatedRigidBodies.push_back(body);
+	return body;
+
+}
+*/
+
+btCollisionObject* btCollisionWorldImporter::getCollisionObjectByName(const char* name)
+{
+	btCollisionObject** bodyPtr = m_nameColObjMap.find(name);
+	if (bodyPtr && *bodyPtr)
+	{
+		return *bodyPtr;
+	}
+	return 0;
+}
+
+btCollisionObject* btCollisionWorldImporter::createCollisionObject(const btTransform& startTransform,btCollisionShape* shape, const char* bodyName)
+{
+	btCollisionObject* colObj = new btCollisionObject();
+	colObj->setWorldTransform(startTransform);
+	colObj->setCollisionShape(shape);
+	m_collisionWorld->addCollisionObject(colObj);//todo: flags etc
+
+	if (bodyName)
+	{
+		char* newname = duplicateName(bodyName);
+		m_objectNameMap.insert(colObj,newname);
+		m_nameColObjMap.insert(newname,colObj);
+	}
+	m_allocatedCollisionObjects.push_back(colObj);
+
+	return colObj;
+}
+
+
+
+btCollisionShape* btCollisionWorldImporter::createPlaneShape(const btVector3& planeNormal,btScalar planeConstant)
+{
+	btStaticPlaneShape* shape = new btStaticPlaneShape(planeNormal,planeConstant);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+btCollisionShape* btCollisionWorldImporter::createBoxShape(const btVector3& halfExtents)
+{
+	btBoxShape* shape = new btBoxShape(halfExtents);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+btCollisionShape* btCollisionWorldImporter::createSphereShape(btScalar radius)
+{
+	btSphereShape* shape = new btSphereShape(radius);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+
+btCollisionShape* btCollisionWorldImporter::createCapsuleShapeX(btScalar radius, btScalar height)
+{
+	btCapsuleShapeX* shape = new btCapsuleShapeX(radius,height);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createCapsuleShapeY(btScalar radius, btScalar height)
+{
+	btCapsuleShape* shape = new btCapsuleShape(radius,height);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createCapsuleShapeZ(btScalar radius, btScalar height)
+{
+	btCapsuleShapeZ* shape = new btCapsuleShapeZ(radius,height);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createCylinderShapeX(btScalar radius,btScalar height)
+{
+	btCylinderShapeX* shape = new btCylinderShapeX(btVector3(height,radius,radius));
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createCylinderShapeY(btScalar radius,btScalar height)
+{
+	btCylinderShape* shape = new btCylinderShape(btVector3(radius,height,radius));
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createCylinderShapeZ(btScalar radius,btScalar height)
+{
+	btCylinderShapeZ* shape = new btCylinderShapeZ(btVector3(radius,radius,height));
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createConeShapeX(btScalar radius,btScalar height)
+{
+	btConeShapeX* shape = new btConeShapeX(radius,height);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createConeShapeY(btScalar radius,btScalar height)
+{
+	btConeShape* shape = new btConeShape(radius,height);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createConeShapeZ(btScalar radius,btScalar height)
+{
+	btConeShapeZ* shape = new btConeShapeZ(radius,height);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btTriangleIndexVertexArray*	btCollisionWorldImporter::createTriangleMeshContainer()
+{
+	btTriangleIndexVertexArray* in = new btTriangleIndexVertexArray();
+	m_allocatedTriangleIndexArrays.push_back(in);
+	return in;
+}
+
+btOptimizedBvh*	btCollisionWorldImporter::createOptimizedBvh()
+{
+	btOptimizedBvh* bvh = new btOptimizedBvh();
+	m_allocatedBvhs.push_back(bvh);
+	return bvh;
+}
+
+
+btTriangleInfoMap* btCollisionWorldImporter::createTriangleInfoMap()
+{
+	btTriangleInfoMap* tim = new btTriangleInfoMap();
+	m_allocatedTriangleInfoMaps.push_back(tim);
+	return tim;
+}
+
+btBvhTriangleMeshShape* btCollisionWorldImporter::createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh)
+{
+	if (bvh)
+	{
+		btBvhTriangleMeshShape* bvhTriMesh = new btBvhTriangleMeshShape(trimesh,bvh->isQuantized(), false);
+		bvhTriMesh->setOptimizedBvh(bvh);
+		m_allocatedCollisionShapes.push_back(bvhTriMesh);
+		return bvhTriMesh;
+	}
+
+	btBvhTriangleMeshShape* ts = new btBvhTriangleMeshShape(trimesh,true);
+	m_allocatedCollisionShapes.push_back(ts);
+	return ts;
+
+}
+btCollisionShape* btCollisionWorldImporter::createConvexTriangleMeshShape(btStridingMeshInterface* trimesh)
+{
+	return 0;
+}
+#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT
+btGImpactMeshShape* btCollisionWorldImporter::createGimpactShape(btStridingMeshInterface* trimesh)
+{
+	btGImpactMeshShape* shape = new btGImpactMeshShape(trimesh);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+
+}
+#endif //SUPPORT_GIMPACT_SHAPE_IMPORT
+
+btConvexHullShape* btCollisionWorldImporter::createConvexHullShape()
+{
+	btConvexHullShape* shape = new btConvexHullShape();
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCompoundShape* btCollisionWorldImporter::createCompoundShape()
+{
+	btCompoundShape* shape = new btCompoundShape();
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+
+btScaledBvhTriangleMeshShape* btCollisionWorldImporter::createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScaling)
+{
+	btScaledBvhTriangleMeshShape* shape = new btScaledBvhTriangleMeshShape(meshShape,localScaling);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btMultiSphereShape* btCollisionWorldImporter::createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres)
+{
+	btMultiSphereShape* shape = new btMultiSphereShape(positions, radi, numSpheres);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+
+
+	// query for data
+int	btCollisionWorldImporter::getNumCollisionShapes() const
+{
+	return m_allocatedCollisionShapes.size();
+}
+
+btCollisionShape* btCollisionWorldImporter::getCollisionShapeByIndex(int index)
+{
+	return m_allocatedCollisionShapes[index];
+}
+
+btCollisionShape* btCollisionWorldImporter::getCollisionShapeByName(const char* name)
+{
+	btCollisionShape** shapePtr = m_nameShapeMap.find(name);
+	if (shapePtr&& *shapePtr)
+	{
+		return *shapePtr;
+	}
+	return 0;
+}
+
+
+const char*	btCollisionWorldImporter::getNameForPointer(const void* ptr) const
+{
+	const char*const * namePtr = m_objectNameMap.find(ptr);
+	if (namePtr && *namePtr)
+		return *namePtr;
+	return 0;
+}
+
+
+int btCollisionWorldImporter::getNumRigidBodies() const
+{
+	return m_allocatedRigidBodies.size();
+}
+
+btCollisionObject* btCollisionWorldImporter::getRigidBodyByIndex(int index) const
+{
+	return m_allocatedRigidBodies[index];
+}
+
+
+int btCollisionWorldImporter::getNumBvhs() const
+{
+	return m_allocatedBvhs.size();
+}
+ btOptimizedBvh* btCollisionWorldImporter::getBvhByIndex(int index) const
+{
+	return m_allocatedBvhs[index];
+}
+
+int btCollisionWorldImporter::getNumTriangleInfoMaps() const
+{
+	return m_allocatedTriangleInfoMaps.size();
+}
+
+btTriangleInfoMap* btCollisionWorldImporter::getTriangleInfoMapByIndex(int index) const
+{
+	return m_allocatedTriangleInfoMaps[index];
+}
+
+

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

@@ -0,0 +1,190 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2014 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_COLLISION_WORLD_IMPORTER_H
+#define BT_COLLISION_WORLD_IMPORTER_H
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btHashMap.h"
+
+class btCollisionShape;
+class btCollisionObject;
+class btBulletSerializedArrays;
+
+
+struct ConstraintInput;
+class btCollisionWorld;
+struct btCollisionShapeData;
+class btTriangleIndexVertexArray;
+class btStridingMeshInterface;
+struct btStridingMeshInterfaceData;
+class btGImpactMeshShape;
+class btOptimizedBvh;
+struct btTriangleInfoMap;
+class btBvhTriangleMeshShape;
+class btPoint2PointConstraint;
+class btHingeConstraint;
+class btConeTwistConstraint;
+class btGeneric6DofConstraint;
+class btGeneric6DofSpringConstraint;
+class btSliderConstraint;
+class btGearConstraint;
+struct btContactSolverInfo;
+
+
+
+
+class btCollisionWorldImporter
+{
+protected:
+	btCollisionWorld* m_collisionWorld;
+
+	int m_verboseMode;
+
+	btAlignedObjectArray<btCollisionShape*>  m_allocatedCollisionShapes;
+	btAlignedObjectArray<btCollisionObject*> m_allocatedRigidBodies;
+
+	btAlignedObjectArray<btOptimizedBvh*>	 m_allocatedBvhs;
+	btAlignedObjectArray<btTriangleInfoMap*> m_allocatedTriangleInfoMaps;
+	btAlignedObjectArray<btTriangleIndexVertexArray*> m_allocatedTriangleIndexArrays;
+	btAlignedObjectArray<btStridingMeshInterfaceData*> m_allocatedbtStridingMeshInterfaceDatas;
+	btAlignedObjectArray<btCollisionObject*> m_allocatedCollisionObjects;
+
+
+	btAlignedObjectArray<char*>				m_allocatedNames;
+
+	btAlignedObjectArray<int*>				m_indexArrays;
+	btAlignedObjectArray<short int*>		m_shortIndexArrays;
+	btAlignedObjectArray<unsigned char*>	m_charIndexArrays;
+
+	btAlignedObjectArray<btVector3FloatData*>	m_floatVertexArrays;
+	btAlignedObjectArray<btVector3DoubleData*>	m_doubleVertexArrays;
+
+
+	btHashMap<btHashPtr,btOptimizedBvh*>	m_bvhMap;
+	btHashMap<btHashPtr,btTriangleInfoMap*>	m_timMap;
+
+	btHashMap<btHashString,btCollisionShape*>	m_nameShapeMap;
+	btHashMap<btHashString,btCollisionObject*>	m_nameColObjMap;
+
+	btHashMap<btHashPtr,const char*>	m_objectNameMap;
+
+	btHashMap<btHashPtr,btCollisionShape*>	m_shapeMap;
+	btHashMap<btHashPtr,btCollisionObject*>	m_bodyMap;
+
+
+	//methods
+
+
+
+	char*	duplicateName(const char* name);
+
+	btCollisionShape* convertCollisionShape(  btCollisionShapeData* shapeData  );
+
+
+public:
+
+	btCollisionWorldImporter(btCollisionWorld* world);
+
+	virtual ~btCollisionWorldImporter();
+
+    bool	convertAllObjects( btBulletSerializedArrays* arrays);
+
+		///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load.
+	///make sure you don't use the dynamics world containing objects after you call this method
+	virtual void deleteAllData();
+
+	void	setVerboseMode(int verboseMode)
+	{
+		m_verboseMode = verboseMode;
+	}
+
+	int getVerboseMode() const
+	{
+		return m_verboseMode;
+	}
+
+		// query for data
+	int	getNumCollisionShapes() const;
+	btCollisionShape* getCollisionShapeByIndex(int index);
+	int getNumRigidBodies() const;
+	btCollisionObject* getRigidBodyByIndex(int index) const;
+	int getNumConstraints() const;
+
+	int getNumBvhs() const;
+	btOptimizedBvh*  getBvhByIndex(int index) const;
+	int getNumTriangleInfoMaps() const;
+	btTriangleInfoMap* getTriangleInfoMapByIndex(int index) const;
+
+	// queris involving named objects
+	btCollisionShape* getCollisionShapeByName(const char* name);
+	btCollisionObject* getCollisionObjectByName(const char* name);
+
+
+	const char*	getNameForPointer(const void* ptr) const;
+
+	///those virtuals are called by load and can be overridden by the user
+
+
+
+	//bodies
+
+	virtual btCollisionObject*  createCollisionObject(	const btTransform& startTransform,	btCollisionShape* shape,const char* bodyName);
+
+	///shapes
+
+	virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal,btScalar planeConstant);
+	virtual btCollisionShape* createBoxShape(const btVector3& halfExtents);
+	virtual btCollisionShape* createSphereShape(btScalar radius);
+	virtual btCollisionShape* createCapsuleShapeX(btScalar radius, btScalar height);
+	virtual btCollisionShape* createCapsuleShapeY(btScalar radius, btScalar height);
+	virtual btCollisionShape* createCapsuleShapeZ(btScalar radius, btScalar height);
+
+	virtual btCollisionShape* createCylinderShapeX(btScalar radius,btScalar height);
+	virtual btCollisionShape* createCylinderShapeY(btScalar radius,btScalar height);
+	virtual btCollisionShape* createCylinderShapeZ(btScalar radius,btScalar height);
+	virtual btCollisionShape* createConeShapeX(btScalar radius,btScalar height);
+	virtual btCollisionShape* createConeShapeY(btScalar radius,btScalar height);
+	virtual btCollisionShape* createConeShapeZ(btScalar radius,btScalar height);
+	virtual class btTriangleIndexVertexArray*	createTriangleMeshContainer();
+	virtual	btBvhTriangleMeshShape* createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh);
+	virtual btCollisionShape* createConvexTriangleMeshShape(btStridingMeshInterface* trimesh);
+#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT
+	virtual btGImpactMeshShape* createGimpactShape(btStridingMeshInterface* trimesh);
+#endif //SUPPORT_GIMPACT_SHAPE_IMPORT
+	virtual btStridingMeshInterfaceData* createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData);
+
+	virtual class btConvexHullShape* createConvexHullShape();
+	virtual class btCompoundShape* createCompoundShape();
+	virtual class btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScalingbtBvhTriangleMeshShape);
+
+	virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres);
+
+	virtual btTriangleIndexVertexArray* createMeshInterface(btStridingMeshInterfaceData& meshData);
+
+	///acceleration and connectivity structures
+	virtual btOptimizedBvh*	createOptimizedBvh();
+	virtual btTriangleInfoMap* createTriangleInfoMap();
+
+
+
+
+};
+
+
+#endif //BT_WORLD_IMPORTER_H

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

@@ -14,8 +14,6 @@ subject to the following restrictions:
 
 */
 
-// Modified by Lasse Oorni for Urho3D
-
 #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
@@ -125,7 +123,7 @@ public:
 
 		//backup
 		btTransform	orgTrans = m_compoundColObjWrap->getWorldTransform();
-		btTransform	orgInterpolationTrans = m_compoundColObjWrap->getWorldTransform();
+		
 		const btTransform& childTrans = compoundShape->getChildTransform(index);
 		btTransform	newChildWorldTrans = orgTrans*childTrans ;
 
@@ -223,10 +221,6 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 	btAssert (colObjWrap->getCollisionShape()->isCompound());
 	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
 
-    // Urho3D: do not go further if compound does not have child shapes, to prevent assert
-    if (!compoundShape->getNumChildShapes())
-        return;
-
 	///btCompoundShape might have changed:
 	////make sure the internal child collision algorithm caches are still valid
 	if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
@@ -235,9 +229,12 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 		removeChildAlgorithms();
 		
 		preallocateChildAlgorithms(body0Wrap,body1Wrap);
+		m_compoundShapeRevision = compoundShape->getUpdateRevision();
 	}
 
-
+    if (m_childCollisionAlgorithms.size()==0)
+        return;
+    
 	const btDbvt* tree = compoundShape->getDynamicAabbTree();
 	//use a dynamic aabb tree to cull potential child-overlaps
 	btCompoundLeafCallback  callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
@@ -297,7 +294,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 		btManifoldArray	manifoldArray;
         const btCollisionShape* childShape = 0;
         btTransform	orgTrans;
-        btTransform	orgInterpolationTrans;
+        
         btTransform	newChildWorldTrans;
         btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;        
         
@@ -307,8 +304,8 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 			{
 				childShape = compoundShape->getChildShape(i);
 			//if not longer overlapping, remove the algorithm
-                orgTrans = colObjWrap->getWorldTransform();
-                orgInterpolationTrans = colObjWrap->getWorldTransform();
+				orgTrans = colObjWrap->getWorldTransform();
+                
 				const btTransform& childTrans = compoundShape->getChildTransform(i);
                 newChildWorldTrans = orgTrans*childTrans ;
 

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

@@ -36,6 +36,7 @@ extern btShapePairCallback gCompoundChildShapePairCallback;
 /// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
 class btCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
 {
+protected:
 	btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
 	bool m_isSwapped;
 

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

@@ -27,10 +27,8 @@ subject to the following restrictions:
 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)
+:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped)
 {
-	m_ownsManifold = false;
 
 	void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16);
 	m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache();
@@ -114,10 +112,9 @@ struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
 									btManifoldResult*	resultOut,
 									btHashedSimplePairCache* childAlgorithmsCache,
 									btPersistentManifold*	sharedManifold)
-		:m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
+		:m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
 		m_childCollisionAlgorithmCache(childAlgorithmsCache),
-		m_sharedManifold(sharedManifold),
-		m_numOverlapPairs(0)
+		m_sharedManifold(sharedManifold)
 	{
 
 	}
@@ -292,12 +289,21 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 	const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
 	const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
 
+	const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
+	const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
+	if (!tree0 || !tree1)
+	{
+		return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut);
+	}
 	///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();
+		m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
+		m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
+
 	}
 
 
@@ -329,8 +335,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 	}
 
 
-	const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
-	const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
+	
 
 	btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);
 

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

@@ -17,6 +17,8 @@ subject to the following restrictions:
 #ifndef BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
 #define BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
 
+#include "btCompoundCollisionAlgorithm.h"
+
 #include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
@@ -35,15 +37,12 @@ typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCol
 extern btShapePairCallback gCompoundCompoundChildShapePairCallback;
 
 /// btCompoundCompoundCollisionAlgorithm  supports collision between two btCompoundCollisionShape shapes
-class btCompoundCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
+class btCompoundCompoundCollisionAlgorithm  : public btCompoundCollisionAlgorithm
 {
 
 	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;

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

@@ -88,20 +88,19 @@ partId, int triangleIndex)
         //just for debugging purposes
         //printf("triangle %d",m_triangleCount++);
 
-        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
 	if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
 	{
+		const btCollisionObject* ob = const_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
 		btVector3 color(1,1,0);
 		btTransform& tr = ob->getWorldTransform();
 		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);

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

@@ -105,8 +105,7 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
 	int maxSize = sizeof(btConvexConvexAlgorithm);
 	int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm);
 	int maxSize3 = sizeof(btCompoundCollisionAlgorithm);
-	int sl = sizeof(btConvexSeparatingDistanceUtil);
-	sl = sizeof(btGjkPairDetector);
+
 	int	collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize);
 	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
 	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);

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

@@ -117,6 +117,7 @@ public:
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
 	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
 
+	SIMD_FORCE_INLINE	void	deSerializeFloat(struct btCapsuleShapeData* dataBuffer);
 
 };
 
@@ -181,4 +182,13 @@ SIMD_FORCE_INLINE	const char*	btCapsuleShape::serialize(void* dataBuffer, btSeri
 	return "btCapsuleShapeData";
 }
 
+SIMD_FORCE_INLINE	void	btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer)
+{
+	m_implicitShapeDimensions.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_implicitShapeDimensions);
+	m_collisionMargin = dataBuffer->m_convexInternalShapeData.m_collisionMargin;
+	m_localScaling.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_localScaling);
+	//it is best to already pre-allocate the matching btCapsuleShape*(X/Z) version to match m_upAxis
+	m_upAxis = dataBuffer->m_upAxis;
+}
+
 #endif //BT_CAPSULE_SHAPE_H

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

@@ -29,12 +29,13 @@ ATTRIBUTE_ALIGNED16(class) btCollisionShape
 protected:
 	int m_shapeType;
 	void* m_userPointer;
+	int m_userIndex;
 
 public:
 
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0)
+	btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1)
 	{
 	}
 
@@ -130,6 +131,16 @@ public:
 	{
 		return m_userPointer;
 	}
+	void setUserIndex(int index)
+	{
+		m_userIndex = index;
+	}
+
+	int getUserIndex() const
+	{
+		return m_userIndex;
+	}
+
 
 	virtual	int	calculateSerializeBufferSize() const;
 

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

@@ -77,8 +77,8 @@ void	btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio
 	if (m_dynamicAabbTree)
 	{
 		const btDbvtVolume	bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
-		int index = m_children.size();
-		child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
+		size_t index = m_children.size();
+		child.m_node = m_dynamicAabbTree->insert(bounds,reinterpret_cast<void*>(index) );
 	}
 
 	m_children.push_back(child);
@@ -312,7 +312,8 @@ void btCompoundShape::createAabbTreeFromChildren()
             child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax);
 
             const btDbvtVolume  bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
-            child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
+			size_t index2 = index;
+            child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2) );
         }
     }
 }

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

@@ -274,7 +274,7 @@ void	btConvexPolyhedron::initialize()
 #endif
 }
 
-void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
+void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
 {
 	minProj = FLT_MAX;
 	maxProj = -FLT_MAX;

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

@@ -56,7 +56,7 @@ ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron
 	void	initialize();
 	bool testContainment() const;
 
-	void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
+	void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
 };
 
 	

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

@@ -365,14 +365,15 @@ void	btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
 			{
         //first triangle
         getVertex(x,j,vertices[0]);
-        getVertex(x+1,j,vertices[1]);
-        getVertex(x+1,j+1,vertices[2]);
+		getVertex(x, j + 1, vertices[1]);
+		getVertex(x + 1, j + 1, vertices[2]);
         callback->processTriangle(vertices,x,j);
         //second triangle
       //  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);				
+		getVertex(x + 1, j, vertices[2]);
+		callback->processTriangle(vertices, x, j);
+
 			} else
 			{
         //first triangle

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

@@ -75,6 +75,13 @@ void	btTriangleMesh::addIndex(int index)
 	}
 }
 
+void	btTriangleMesh::addTriangleIndices(int index1, int index2, int index3 )
+{
+	m_indexedMeshes[0].m_numTriangles++;
+	addIndex( index1 );
+	addIndex( index2 );
+	addIndex( index3 );
+}
 
 int	btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices)
 {

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

@@ -52,7 +52,10 @@ class btTriangleMesh : public btTriangleIndexVertexArray
 		///By default addTriangle won't search for duplicate vertices, because the search is very slow for large triangle meshes.
 		///In general it is better to directly use btTriangleIndexVertexArray instead.
 		void	addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2, bool removeDuplicateVertices=false);
-		
+
+		///Add a triangle using its indices. Make sure the indices are pointing within the vertices array, so add the vertices first (and to be sure, avoid removal of duplicate vertices)	
+		void	addTriangleIndices(int index1, int index2, int index3 );
+	
 		int getNumTriangles() const;
 
 		virtual void	preallocateVertices(int numverts);

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

@@ -404,12 +404,12 @@ SIMD_FORCE_INLINE void SEGMENT_COLLISION(
 	CLASS_POINT & vPointA,
 	CLASS_POINT & vPointB)
 {
-    CLASS_POINT _AD,_BD,_N;
+    CLASS_POINT _AD,_BD,n;
     vec4f _M;//plane
     VEC_DIFF(_AD,vA2,vA1);
     VEC_DIFF(_BD,vB2,vB1);
-    VEC_CROSS(_N,_AD,_BD);
-    GREAL _tp = VEC_DOT(_N,_N);
+    VEC_CROSS(n,_AD,_BD);
+    GREAL _tp = VEC_DOT(n,n);
     if(_tp<G_EPSILON)//ARE PARALELE
     {
     	//project B over A
@@ -424,10 +424,10 @@ SIMD_FORCE_INLINE void SEGMENT_COLLISION(
     	_M[2] = VEC_DOT(vA1,_AD);
     	_M[3] = VEC_DOT(vA2,_AD);
     	//mid points
-    	_N[0] = (_M[0]+_M[1])*0.5f;
-    	_N[1] = (_M[2]+_M[3])*0.5f;
+    	n[0] = (_M[0]+_M[1])*0.5f;
+    	n[1] = (_M[2]+_M[3])*0.5f;
 
-    	if(_N[0]<_N[1])
+    	if(n[0]<n[1])
     	{
     		if(_M[1]<_M[2])
     		{
@@ -467,7 +467,7 @@ SIMD_FORCE_INLINE void SEGMENT_COLLISION(
     }
 
 
-    VEC_CROSS(_M,_N,_BD);
+    VEC_CROSS(_M,n,_BD);
     _M[3] = VEC_DOT(_M,vB1);
 
     LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1));

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

@@ -0,0 +1,369 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2014 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_GJK_EPA_PENETATION_CONVEX_COLLISION_H
+#define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
+
+#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision...
+#include "btGjkEpa3.h"
+#include "btGjkCollisionDescription.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+
+
+
+
+
+
+template <typename btConvexTemplate>
+bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b,
+                          const btGjkCollisionDescription& colDesc,
+                          btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB)
+{
+    (void)v;
+    
+    //	const btScalar				radialmargin(btScalar(0.));
+    
+    btVector3	guessVector(b.getWorldTransform().getOrigin()-a.getWorldTransform().getOrigin());//?? why not use the GJK input?
+    
+    btGjkEpaSolver3::sResults	results;
+
+    
+    if(btGjkEpaSolver3_Penetration(a,b,guessVector,results))
+        
+    {
+        //	debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
+        //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
+        wWitnessOnA = results.witnesses[0];
+        wWitnessOnB = results.witnesses[1];
+        v = results.normal;
+        return true;
+    } else
+    {
+        if(btGjkEpaSolver3_Distance(a,b,guessVector,results))
+        {
+            wWitnessOnA = results.witnesses[0];
+            wWitnessOnB = results.witnesses[1];
+            v = results.normal;
+            return false;
+        }
+    }
+    return false;
+}
+
+template <typename btConvexTemplate, typename btGjkDistanceTemplate>
+int	btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo)
+{
+    
+    bool m_catchDegeneracies  = true;
+    btScalar m_cachedSeparatingDistance = 0.f;
+    
+    btScalar distance=btScalar(0.);
+    btVector3	normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
+    
+    btVector3 pointOnA,pointOnB;
+    btTransform	localTransA = a.getWorldTransform();
+    btTransform localTransB = b.getWorldTransform();
+    
+    btScalar marginA = a.getMargin();
+    btScalar marginB = b.getMargin();
+    
+    int m_curIter = 0;
+    int gGjkMaxIter = colDesc.m_maxGjkIterations;//this is to catch invalid input, perhaps check for #NaN?
+    btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir;
+    
+    bool isValid = false;
+    bool checkSimplex = false;
+    bool checkPenetration = true;
+    int m_degenerateSimplex = 0;
+    
+    int m_lastUsedMethod = -1;
+    
+    {
+        btScalar squaredDistance = BT_LARGE_FLOAT;
+        btScalar delta = btScalar(0.);
+        
+        btScalar margin = marginA + marginB;
+        
+        
+        
+        simplexSolver.reset();
+        
+        for ( ; ; )
+            //while (true)
+        {
+            
+            btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis();
+            btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis();
+            
+            btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA);
+            btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB);
+            
+            btVector3  pWorld = localTransA(pInA);
+            btVector3  qWorld = localTransB(qInB);
+            
+            
+            
+            btVector3 w	= pWorld - qWorld;
+            delta = m_cachedSeparatingAxis.dot(w);
+            
+            // potential exit, they don't overlap
+            if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared))
+            {
+                m_degenerateSimplex = 10;
+                checkSimplex=true;
+                //checkPenetration = false;
+                break;
+            }
+            
+            //exit 0: the new point is already in the simplex, or we didn't come any closer
+            if (simplexSolver.inSimplex(w))
+            {
+                m_degenerateSimplex = 1;
+                checkSimplex = true;
+                break;
+            }
+            // are we getting any closer ?
+            btScalar f0 = squaredDistance - delta;
+            btScalar f1 = squaredDistance * colDesc.m_gjkRelError2;
+            
+            if (f0 <= f1)
+            {
+                if (f0 <= btScalar(0.))
+                {
+                    m_degenerateSimplex = 2;
+                } else
+                {
+                    m_degenerateSimplex = 11;
+                }
+                checkSimplex = true;
+                break;
+            }
+            
+            //add current vertex to simplex
+            simplexSolver.addVertex(w, pWorld, qWorld);
+            btVector3 newCachedSeparatingAxis;
+            
+            //calculate the closest point to the origin (update vector v)
+            if (!simplexSolver.closest(newCachedSeparatingAxis))
+            {
+                m_degenerateSimplex = 3;
+                checkSimplex = true;
+                break;
+            }
+            
+            if(newCachedSeparatingAxis.length2()<colDesc.m_gjkRelError2)
+            {
+                m_cachedSeparatingAxis = newCachedSeparatingAxis;
+                m_degenerateSimplex = 6;
+                checkSimplex = true;
+                break;
+            }
+            
+            btScalar previousSquaredDistance = squaredDistance;
+            squaredDistance = newCachedSeparatingAxis.length2();
+#if 0
+            ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
+            if (squaredDistance>previousSquaredDistance)
+            {
+                m_degenerateSimplex = 7;
+                squaredDistance = previousSquaredDistance;
+                checkSimplex = false;
+                break;
+            }
+#endif //
+            
+            
+            //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
+            
+            //are we getting any closer ?
+            if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
+            {
+                //				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+                checkSimplex = true;
+                m_degenerateSimplex = 12;
+                
+                break;
+            }
+            
+            m_cachedSeparatingAxis = newCachedSeparatingAxis;
+            
+            //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
+            if (m_curIter++ > gGjkMaxIter)
+            {
+#if defined(DEBUG) || defined (_DEBUG)
+                
+                printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
+                printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n",
+                       m_cachedSeparatingAxis.getX(),
+                       m_cachedSeparatingAxis.getY(),
+                       m_cachedSeparatingAxis.getZ(),
+                       squaredDistance);
+#endif
+                
+                break;
+                
+            }
+            
+            
+            bool check = (!simplexSolver.fullSimplex());
+            //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
+            
+            if (!check)
+            {
+                //do we need this backup_closest here ?
+                //				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+                m_degenerateSimplex = 13;
+                break;
+            }
+        }
+        
+        if (checkSimplex)
+        {
+            simplexSolver.compute_points(pointOnA, pointOnB);
+            normalInB = m_cachedSeparatingAxis;
+            
+            btScalar lenSqr =m_cachedSeparatingAxis.length2();
+            
+            //valid normal
+            if (lenSqr < 0.0001)
+            {
+                m_degenerateSimplex = 5;
+            }
+            if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
+            {
+                btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+                normalInB *= rlen; //normalize
+                
+                btScalar s = btSqrt(squaredDistance);
+                
+                btAssert(s > btScalar(0.0));
+                pointOnA -= m_cachedSeparatingAxis * (marginA / s);
+                pointOnB += m_cachedSeparatingAxis * (marginB / s);
+                distance = ((btScalar(1.)/rlen) - margin);
+                isValid = true;
+                
+                m_lastUsedMethod = 1;
+            } else
+            {
+                m_lastUsedMethod = 2;
+            }
+        }
+        
+        bool catchDegeneratePenetrationCase =
+        (m_catchDegeneracies &&  m_degenerateSimplex && ((distance+margin) < 0.01));
+        
+        //if (checkPenetration && !isValid)
+        if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
+        {
+            //penetration case
+            
+            //if there is no way to handle penetrations, bail out
+            
+            // Penetration depth case.
+            btVector3 tmpPointOnA,tmpPointOnB;
+            
+            m_cachedSeparatingAxis.setZero();
+            
+            bool isValid2 = btGjkEpaCalcPenDepth(a,b,
+                                                 colDesc,
+                                                 m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);
+            
+            if (isValid2)
+            {
+                btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
+                btScalar lenSqr = tmpNormalInB.length2();
+                if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
+                {
+                    tmpNormalInB = m_cachedSeparatingAxis;
+                    lenSqr = m_cachedSeparatingAxis.length2();
+                }
+                
+                if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+                {
+                    tmpNormalInB /= btSqrt(lenSqr);
+                    btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
+                    //only replace valid penetrations when the result is deeper (check)
+                    if (!isValid || (distance2 < distance))
+                    {
+                        distance = distance2;
+                        pointOnA = tmpPointOnA;
+                        pointOnB = tmpPointOnB;
+                        normalInB = tmpNormalInB;
+                        
+                        isValid = true;
+                        m_lastUsedMethod = 3;
+                    } else
+                    {
+                        m_lastUsedMethod = 8;
+                    }
+                } else
+                {
+                    m_lastUsedMethod = 9;
+                }
+            } else
+                
+            {
+                ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
+                ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
+                ///reports a valid positive distance. Use the results of the second GJK instead of failing.
+                ///thanks to Jacob.Langford for the reproduction case
+                ///http://code.google.com/p/bullet/issues/detail?id=250
+                
+                
+                if (m_cachedSeparatingAxis.length2() > btScalar(0.))
+                {
+                    btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
+                    //only replace valid distances when the distance is less
+                    if (!isValid || (distance2 < distance))
+                    {
+                        distance = distance2;
+                        pointOnA = tmpPointOnA;
+                        pointOnB = tmpPointOnB;
+                        pointOnA -= m_cachedSeparatingAxis * marginA ;
+                        pointOnB += m_cachedSeparatingAxis * marginB ;
+                        normalInB = m_cachedSeparatingAxis;
+                        normalInB.normalize();
+                        
+                        isValid = true;
+                        m_lastUsedMethod = 6;
+                    } else
+                    {
+                        m_lastUsedMethod = 5;
+                    }
+                }
+            }
+        }
+    }
+    
+    
+    
+    if (isValid && ((distance < 0) || (distance*distance < colDesc.m_maximumDistanceSquared)))
+    {
+        
+        m_cachedSeparatingAxis = normalInB;
+        m_cachedSeparatingDistance = distance;
+        distInfo->m_distance = distance;
+        distInfo->m_normalBtoA = normalInB;
+        distInfo->m_pointOnB = pointOnB;
+        distInfo->m_pointOnA = pointOnB+normalInB*distance;
+        return 0;
+    }
+    return -m_lastUsedMethod;
+}
+
+
+
+
+#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H

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

@@ -0,0 +1,41 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2014 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 GJK_COLLISION_DESCRIPTION_H
+#define GJK_COLLISION_DESCRIPTION_H
+
+#include "LinearMath/btVector3.h"
+
+struct btGjkCollisionDescription
+{
+    btVector3	m_firstDir;
+    int			m_maxGjkIterations;
+    btScalar	m_maximumDistanceSquared;
+    btScalar	m_gjkRelError2;
+    btGjkCollisionDescription()
+    :m_firstDir(0,1,0),
+    m_maxGjkIterations(1000),
+    m_maximumDistanceSquared(1e30f),
+    m_gjkRelError2(1.0e-6)
+    {
+    }
+    virtual ~btGjkCollisionDescription()
+    {
+    }
+};
+
+#endif //GJK_COLLISION_DESCRIPTION_H
+

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

@@ -0,0 +1,1035 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2014 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.
+*/
+
+/*
+Initial GJK-EPA collision solver by Nathanael Presson, 2008
+Improvements and refactoring by Erwin Coumans, 2008-2014
+*/
+#ifndef BT_GJK_EPA3_H
+#define BT_GJK_EPA3_H
+
+#include "LinearMath/btTransform.h"
+#include "btGjkCollisionDescription.h"
+
+
+
+struct	btGjkEpaSolver3
+{
+struct	sResults
+	{
+	enum eStatus
+		{
+		Separated,		/* Shapes doesnt penetrate												*/ 
+		Penetrating,	/* Shapes are penetrating												*/ 
+		GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/ 
+		EPA_Failed		/* EPA phase fail, bigger problem, need to save parameters, and debug	*/ 
+		}		status;
+	btVector3	witnesses[2];
+	btVector3	normal;
+	btScalar	distance;
+	};
+
+
+};
+
+
+
+#if defined(DEBUG) || defined (_DEBUG)
+#include <stdio.h> //for debug printf
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+#endif //__SPU__
+#endif
+
+
+    
+    // Config
+    
+    /* GJK	*/
+#define GJK_MAX_ITERATIONS	128
+#define GJK_ACCURARY		((btScalar)0.0001)
+#define GJK_MIN_DISTANCE	((btScalar)0.0001)
+#define GJK_DUPLICATED_EPS	((btScalar)0.0001)
+#define GJK_SIMPLEX2_EPS	((btScalar)0.0)
+#define GJK_SIMPLEX3_EPS	((btScalar)0.0)
+#define GJK_SIMPLEX4_EPS	((btScalar)0.0)
+    
+    /* EPA	*/
+#define EPA_MAX_VERTICES	64
+#define EPA_MAX_FACES		(EPA_MAX_VERTICES*2)
+#define EPA_MAX_ITERATIONS	255
+#define EPA_ACCURACY		((btScalar)0.0001)
+#define EPA_FALLBACK		(10*EPA_ACCURACY)
+#define EPA_PLANE_EPS		((btScalar)0.00001)
+#define EPA_INSIDE_EPS		((btScalar)0.01)
+    
+    
+    // Shorthands
+    typedef unsigned int	U;
+    typedef unsigned char	U1;
+    
+    // MinkowskiDiff
+    template <typename btConvexTemplate>
+    struct	MinkowskiDiff
+    {
+        const btConvexTemplate* m_convexAPtr;
+        const btConvexTemplate* m_convexBPtr;
+        
+        btMatrix3x3				m_toshape1;
+        btTransform				m_toshape0;
+        
+        bool					m_enableMargin;
+        
+        
+        MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b)
+        :m_convexAPtr(&a),
+        m_convexBPtr(&b)
+        {
+        }
+        
+        void					EnableMargin(bool enable)
+        {
+            m_enableMargin = enable;
+        }
+        inline btVector3		Support0(const btVector3& d) const
+        {
+            return m_convexAPtr->getLocalSupportWithMargin(d);
+        }
+        inline btVector3		Support1(const btVector3& d) const
+        {
+            return m_toshape0*m_convexBPtr->getLocalSupportWithMargin(m_toshape1*d);
+        }
+        
+        
+        inline btVector3		Support(const btVector3& d) const
+        {
+            return(Support0(d)-Support1(-d));
+        }
+        btVector3				Support(const btVector3& d,U index) const
+        {
+            if(index)
+                return(Support1(d));
+            else
+                return(Support0(d));
+        }
+    };
+    
+enum	eGjkStatus
+{
+    eGjkValid,
+    eGjkInside,
+    eGjkFailed
+};
+
+    // GJK
+    template <typename btConvexTemplate>
+    struct	GJK
+    {
+        /* Types		*/
+        struct	sSV
+        {
+            btVector3	d,w;
+        };
+        struct	sSimplex
+        {
+            sSV*		c[4];
+            btScalar	p[4];
+            U			rank;
+        };
+        
+        /* Fields		*/
+        
+        MinkowskiDiff<btConvexTemplate>			m_shape;
+        btVector3		m_ray;
+        btScalar		m_distance;
+        sSimplex		m_simplices[2];
+        sSV				m_store[4];
+        sSV*			m_free[4];
+        U				m_nfree;
+        U				m_current;
+        sSimplex*		m_simplex;
+        eGjkStatus      m_status;
+        /* Methods		*/
+        
+        GJK(const btConvexTemplate& a, const btConvexTemplate& b)
+        :m_shape(a,b)
+        {
+            Initialize();
+        }
+        void				Initialize()
+        {
+            m_ray		=	btVector3(0,0,0);
+            m_nfree		=	0;
+            m_status	=	eGjkFailed;
+            m_current	=	0;
+            m_distance	=	0;
+        }
+        eGjkStatus			Evaluate(const MinkowskiDiff<btConvexTemplate>& shapearg,const btVector3& guess)
+        {
+            U			iterations=0;
+            btScalar	sqdist=0;
+            btScalar	alpha=0;
+            btVector3	lastw[4];
+            U			clastw=0;
+            /* Initialize solver		*/
+            m_free[0]			=	&m_store[0];
+            m_free[1]			=	&m_store[1];
+            m_free[2]			=	&m_store[2];
+            m_free[3]			=	&m_store[3];
+            m_nfree				=	4;
+            m_current			=	0;
+            m_status			=	eGjkValid;
+            m_shape				=	shapearg;
+            m_distance			=	0;
+            /* Initialize simplex		*/
+            m_simplices[0].rank	=	0;
+            m_ray				=	guess;
+            const btScalar	sqrl=	m_ray.length2();
+            appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
+            m_simplices[0].p[0]	=	1;
+            m_ray				=	m_simplices[0].c[0]->w;
+            sqdist				=	sqrl;
+            lastw[0]			=
+            lastw[1]			=
+            lastw[2]			=
+            lastw[3]			=	m_ray;
+            /* Loop						*/
+            do	{
+                const U		next=1-m_current;
+                sSimplex&	cs=m_simplices[m_current];
+                sSimplex&	ns=m_simplices[next];
+                /* Check zero							*/
+                const btScalar	rl=m_ray.length();
+                if(rl<GJK_MIN_DISTANCE)
+                {/* Touching or inside				*/
+                    m_status=eGjkInside;
+                    break;
+                }
+                /* Append new vertice in -'v' direction	*/
+                appendvertice(cs,-m_ray);
+                const btVector3&	w=cs.c[cs.rank-1]->w;
+                bool				found=false;
+                for(U i=0;i<4;++i)
+                {
+                    if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
+                    { found=true;break; }
+                }
+                if(found)
+                {/* Return old simplex				*/
+                    removevertice(m_simplices[m_current]);
+                    break;
+                }
+                else
+                {/* Update lastw					*/
+                    lastw[clastw=(clastw+1)&3]=w;
+                }
+                /* Check for termination				*/
+                const btScalar	omega=btDot(m_ray,w)/rl;
+                alpha=btMax(omega,alpha);
+                if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
+                {/* Return old simplex				*/
+                    removevertice(m_simplices[m_current]);
+                    break;
+                }
+                /* Reduce simplex						*/
+                btScalar	weights[4];
+                U			mask=0;
+                switch(cs.rank)
+                {
+                    case	2:	sqdist=projectorigin(	cs.c[0]->w,
+                                                     cs.c[1]->w,
+                                                     weights,mask);break;
+                    case	3:	sqdist=projectorigin(	cs.c[0]->w,
+                                                     cs.c[1]->w,
+                                                     cs.c[2]->w,
+                                                     weights,mask);break;
+                    case	4:	sqdist=projectorigin(	cs.c[0]->w,
+                                                     cs.c[1]->w,
+                                                     cs.c[2]->w,
+                                                     cs.c[3]->w,
+                                                     weights,mask);break;
+                }
+                if(sqdist>=0)
+                {/* Valid	*/
+                    ns.rank		=	0;
+                    m_ray		=	btVector3(0,0,0);
+                    m_current	=	next;
+                    for(U i=0,ni=cs.rank;i<ni;++i)
+                    {
+                        if(mask&(1<<i))
+                        {
+                            ns.c[ns.rank]		=	cs.c[i];
+                            ns.p[ns.rank++]		=	weights[i];
+                            m_ray				+=	cs.c[i]->w*weights[i];
+                        }
+                        else
+                        {
+                            m_free[m_nfree++]	=	cs.c[i];
+                        }
+                    }
+                    if(mask==15) m_status=eGjkInside;
+                }
+                else
+                {/* Return old simplex				*/
+                    removevertice(m_simplices[m_current]);
+                    break;
+                }
+                m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eGjkFailed;
+            } while(m_status==eGjkValid);
+            m_simplex=&m_simplices[m_current];
+            switch(m_status)
+            {
+                case	eGjkValid:		m_distance=m_ray.length();break;
+                case	eGjkInside:	m_distance=0;break;
+                default:
+                {
+                }
+            }
+            return(m_status);
+        }
+        bool					EncloseOrigin()
+        {
+            switch(m_simplex->rank)
+            {
+                case	1:
+                {
+                    for(U i=0;i<3;++i)
+                    {
+                        btVector3		axis=btVector3(0,0,0);
+                        axis[i]=1;
+                        appendvertice(*m_simplex, axis);
+                        if(EncloseOrigin())	return(true);
+                        removevertice(*m_simplex);
+                        appendvertice(*m_simplex,-axis);
+                        if(EncloseOrigin())	return(true);
+                        removevertice(*m_simplex);
+                    }
+                }
+                    break;
+                case	2:
+                {
+                    const btVector3	d=m_simplex->c[1]->w-m_simplex->c[0]->w;
+                    for(U i=0;i<3;++i)
+                    {
+                        btVector3		axis=btVector3(0,0,0);
+                        axis[i]=1;
+                        const btVector3	p=btCross(d,axis);
+                        if(p.length2()>0)
+                        {
+                            appendvertice(*m_simplex, p);
+                            if(EncloseOrigin())	return(true);
+                            removevertice(*m_simplex);
+                            appendvertice(*m_simplex,-p);
+                            if(EncloseOrigin())	return(true);
+                            removevertice(*m_simplex);
+                        }
+                    }
+                }
+                    break;
+                case	3:
+                {
+                    const btVector3	n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
+                                              m_simplex->c[2]->w-m_simplex->c[0]->w);
+                    if(n.length2()>0)
+                    {
+                        appendvertice(*m_simplex,n);
+                        if(EncloseOrigin())	return(true);
+                        removevertice(*m_simplex);
+                        appendvertice(*m_simplex,-n);
+                        if(EncloseOrigin())	return(true);
+                        removevertice(*m_simplex);
+                    }
+                }
+                    break;
+                case	4:
+                {
+                    if(btFabs(det(	m_simplex->c[0]->w-m_simplex->c[3]->w,
+                                  m_simplex->c[1]->w-m_simplex->c[3]->w,
+                                  m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
+                        return(true);
+                }
+                    break;
+            }
+            return(false);
+        }
+        /* Internals	*/
+        void				getsupport(const btVector3& d,sSV& sv) const
+        {
+            sv.d	=	d/d.length();
+            sv.w	=	m_shape.Support(sv.d);
+        }
+        void				removevertice(sSimplex& simplex)
+        {
+            m_free[m_nfree++]=simplex.c[--simplex.rank];
+        }
+        void				appendvertice(sSimplex& simplex,const btVector3& v)
+        {
+            simplex.p[simplex.rank]=0;
+            simplex.c[simplex.rank]=m_free[--m_nfree];
+            getsupport(v,*simplex.c[simplex.rank++]);
+        }
+        static btScalar		det(const btVector3& a,const btVector3& b,const btVector3& c)
+        {
+            return(	a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
+                   a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
+                   a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
+        }
+        static btScalar		projectorigin(	const btVector3& a,
+                                          const btVector3& b,
+                                          btScalar* w,U& m)
+        {
+            const btVector3	d=b-a;
+            const btScalar	l=d.length2();
+            if(l>GJK_SIMPLEX2_EPS)
+            {
+                const btScalar	t(l>0?-btDot(a,d)/l:0);
+                if(t>=1)		{ w[0]=0;w[1]=1;m=2;return(b.length2()); }
+                else if(t<=0)	{ w[0]=1;w[1]=0;m=1;return(a.length2()); }
+                else			{ w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
+            }
+            return(-1);
+        }
+        static btScalar		projectorigin(	const btVector3& a,
+                                          const btVector3& b,
+                                          const btVector3& c,
+                                          btScalar* w,U& m)
+        {
+            static const U		imd3[]={1,2,0};
+            const btVector3*	vt[]={&a,&b,&c};
+            const btVector3		dl[]={a-b,b-c,c-a};
+            const btVector3		n=btCross(dl[0],dl[1]);
+            const btScalar		l=n.length2();
+            if(l>GJK_SIMPLEX3_EPS)
+            {
+                btScalar	mindist=-1;
+                btScalar	subw[2]={0.f,0.f};
+                U			subm(0);
+                for(U i=0;i<3;++i)
+                {
+                    if(btDot(*vt[i],btCross(dl[i],n))>0)
+                    {
+                        const U			j=imd3[i];
+                        const btScalar	subd(projectorigin(*vt[i],*vt[j],subw,subm));
+                        if((mindist<0)||(subd<mindist))
+                        {
+                            mindist		=	subd;
+                            m			=	static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
+                            w[i]		=	subw[0];
+                            w[j]		=	subw[1];
+                            w[imd3[j]]	=	0;
+                        }
+                    }
+                }
+                if(mindist<0)
+                {
+                    const btScalar	d=btDot(a,n);
+                    const btScalar	s=btSqrt(l);
+                    const btVector3	p=n*(d/l);
+                    mindist	=	p.length2();
+                    m		=	7;
+                    w[0]	=	(btCross(dl[1],b-p)).length()/s;
+                    w[1]	=	(btCross(dl[2],c-p)).length()/s;
+                    w[2]	=	1-(w[0]+w[1]);
+                }
+                return(mindist);
+            }
+            return(-1);
+        }
+        static btScalar		projectorigin(	const btVector3& a,
+                                          const btVector3& b,
+                                          const btVector3& c,
+                                          const btVector3& d,
+                                          btScalar* w,U& m)
+        {
+            static const U		imd3[]={1,2,0};
+            const btVector3*	vt[]={&a,&b,&c,&d};
+            const btVector3		dl[]={a-d,b-d,c-d};
+            const btScalar		vl=det(dl[0],dl[1],dl[2]);
+            const bool			ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
+            if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
+            {
+                btScalar	mindist=-1;
+                btScalar	subw[3]={0.f,0.f,0.f};
+                U			subm(0);
+                for(U i=0;i<3;++i)
+                {
+                    const U			j=imd3[i];
+                    const btScalar	s=vl*btDot(d,btCross(dl[i],dl[j]));
+                    if(s>0)
+                    {
+                        const btScalar	subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
+                        if((mindist<0)||(subd<mindist))
+                        {
+                            mindist		=	subd;
+                            m			=	static_cast<U>((subm&1?1<<i:0)+
+                                                           (subm&2?1<<j:0)+
+                                                           (subm&4?8:0));
+                            w[i]		=	subw[0];
+                            w[j]		=	subw[1];
+                            w[imd3[j]]	=	0;
+                            w[3]		=	subw[2];
+                        }
+                    }
+                }
+                if(mindist<0)
+                {
+                    mindist	=	0;
+                    m		=	15;
+                    w[0]	=	det(c,b,d)/vl;
+                    w[1]	=	det(a,c,d)/vl;
+                    w[2]	=	det(b,a,d)/vl;
+                    w[3]	=	1-(w[0]+w[1]+w[2]);
+                }
+                return(mindist);
+            }
+            return(-1);
+        }
+    };
+
+
+enum	eEpaStatus
+{
+    eEpaValid,
+    eEpaTouching,
+    eEpaDegenerated,
+    eEpaNonConvex,
+    eEpaInvalidHull,
+    eEpaOutOfFaces,
+    eEpaOutOfVertices,
+    eEpaAccuraryReached,
+    eEpaFallBack,
+    eEpaFailed
+};
+
+
+    // EPA
+template <typename btConvexTemplate>
+    struct	EPA
+    {
+        /* Types		*/
+       
+        struct	sFace
+        {
+            btVector3	n;
+            btScalar	d;
+            typename GJK<btConvexTemplate>::sSV*		c[3];
+            sFace*		f[3];
+            sFace*		l[2];
+            U1			e[3];
+            U1			pass;
+        };
+        struct	sList
+        {
+            sFace*		root;
+            U			count;
+            sList() : root(0),count(0)	{}
+        };
+        struct	sHorizon
+        {
+            sFace*		cf;
+            sFace*		ff;
+            U			nf;
+            sHorizon() : cf(0),ff(0),nf(0)	{}
+        };
+       
+        /* Fields		*/
+        eEpaStatus		m_status;
+        typename GJK<btConvexTemplate>::sSimplex	m_result;
+        btVector3		m_normal;
+        btScalar		m_depth;
+        typename GJK<btConvexTemplate>::sSV				m_sv_store[EPA_MAX_VERTICES];
+        sFace			m_fc_store[EPA_MAX_FACES];
+        U				m_nextsv;
+        sList			m_hull;
+        sList			m_stock;
+        /* Methods		*/
+        EPA()
+        {
+            Initialize();
+        }
+        
+        
+        static inline void		bind(sFace* fa,U ea,sFace* fb,U eb)
+        {
+            fa->e[ea]=(U1)eb;fa->f[ea]=fb;
+            fb->e[eb]=(U1)ea;fb->f[eb]=fa;
+        }
+        static inline void		append(sList& list,sFace* face)
+        {
+            face->l[0]	=	0;
+            face->l[1]	=	list.root;
+            if(list.root) list.root->l[0]=face;
+            list.root	=	face;
+            ++list.count;
+        }
+        static inline void		remove(sList& list,sFace* face)
+        {
+            if(face->l[1]) face->l[1]->l[0]=face->l[0];
+            if(face->l[0]) face->l[0]->l[1]=face->l[1];
+            if(face==list.root) list.root=face->l[1];
+            --list.count;
+        }
+        
+        
+        void				Initialize()
+        {
+            m_status	=	eEpaFailed;
+            m_normal	=	btVector3(0,0,0);
+            m_depth		=	0;
+            m_nextsv	=	0;
+            for(U i=0;i<EPA_MAX_FACES;++i)
+            {
+                append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
+            }
+        }
+        eEpaStatus			Evaluate(GJK<btConvexTemplate>& gjk,const btVector3& guess)
+        {
+            typename GJK<btConvexTemplate>::sSimplex&	simplex=*gjk.m_simplex;
+            if((simplex.rank>1)&&gjk.EncloseOrigin())
+            {
+                
+                /* Clean up				*/
+                while(m_hull.root)
+                {
+                    sFace*	f = m_hull.root;
+                    remove(m_hull,f);
+                    append(m_stock,f);
+                }
+                m_status	=	eEpaValid;
+                m_nextsv	=	0;
+                /* Orient simplex		*/
+                if(gjk.det(	simplex.c[0]->w-simplex.c[3]->w,
+                           simplex.c[1]->w-simplex.c[3]->w,
+                           simplex.c[2]->w-simplex.c[3]->w)<0)
+                {
+                    btSwap(simplex.c[0],simplex.c[1]);
+                    btSwap(simplex.p[0],simplex.p[1]);
+                }
+                /* Build initial hull	*/
+                sFace*	tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
+                    newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
+                    newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
+                    newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
+                if(m_hull.count==4)
+                {
+                    sFace*		best=findbest();
+                    sFace		outer=*best;
+                    U			pass=0;
+                    U			iterations=0;
+                    bind(tetra[0],0,tetra[1],0);
+                    bind(tetra[0],1,tetra[2],0);
+                    bind(tetra[0],2,tetra[3],0);
+                    bind(tetra[1],1,tetra[3],2);
+                    bind(tetra[1],2,tetra[2],1);
+                    bind(tetra[2],2,tetra[3],1);
+                    m_status=eEpaValid;
+                    for(;iterations<EPA_MAX_ITERATIONS;++iterations)
+                    {
+                        if(m_nextsv<EPA_MAX_VERTICES)
+                        {
+                            sHorizon		horizon;
+                            typename GJK<btConvexTemplate>::sSV*			w=&m_sv_store[m_nextsv++];
+                            bool			valid=true;
+                            best->pass	=	(U1)(++pass);
+                            gjk.getsupport(best->n,*w);
+                            const btScalar	wdist=btDot(best->n,w->w)-best->d;
+                            if(wdist>EPA_ACCURACY)
+                            {
+                                for(U j=0;(j<3)&&valid;++j)
+                                {
+                                    valid&=expand(	pass,w,
+                                                  best->f[j],best->e[j],
+                                                  horizon);
+                                }
+                                if(valid&&(horizon.nf>=3))
+                                {
+                                    bind(horizon.cf,1,horizon.ff,2);
+                                    remove(m_hull,best);
+                                    append(m_stock,best);
+                                    best=findbest();
+                                    outer=*best;
+                                } else { m_status=eEpaInvalidHull;break; }
+                            } else { m_status=eEpaAccuraryReached;break; }
+                        } else { m_status=eEpaOutOfVertices;break; }
+                    }
+                    const btVector3	projection=outer.n*outer.d;
+                    m_normal	=	outer.n;
+                    m_depth		=	outer.d;
+                    m_result.rank	=	3;
+                    m_result.c[0]	=	outer.c[0];
+                    m_result.c[1]	=	outer.c[1];
+                    m_result.c[2]	=	outer.c[2];
+                    m_result.p[0]	=	btCross(	outer.c[1]->w-projection,
+                                                outer.c[2]->w-projection).length();
+                    m_result.p[1]	=	btCross(	outer.c[2]->w-projection,
+                                                outer.c[0]->w-projection).length();
+                    m_result.p[2]	=	btCross(	outer.c[0]->w-projection,
+                                                outer.c[1]->w-projection).length();
+                    const btScalar	sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
+                    m_result.p[0]	/=	sum;
+                    m_result.p[1]	/=	sum;
+                    m_result.p[2]	/=	sum;
+                    return(m_status);
+                }
+            }
+            /* Fallback		*/
+            m_status	=	eEpaFallBack;
+            m_normal	=	-guess;
+            const btScalar	nl=m_normal.length();
+            if(nl>0)
+                m_normal	=	m_normal/nl;
+            else
+                m_normal	=	btVector3(1,0,0);
+            m_depth	=	0;
+            m_result.rank=1;
+            m_result.c[0]=simplex.c[0];
+            m_result.p[0]=1;
+            return(m_status);
+        }
+        bool getedgedist(sFace* face, typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::sSV* b, btScalar& dist)
+        {
+            const btVector3 ba = b->w - a->w;
+            const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane
+            const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required
+            
+            if(a_dot_nab < 0)
+            {
+                // Outside of edge a->b
+                
+                const btScalar ba_l2 = ba.length2();
+                const btScalar a_dot_ba = btDot(a->w, ba);
+                const btScalar b_dot_ba = btDot(b->w, ba);
+                
+                if(a_dot_ba > 0)
+                {
+                    // Pick distance vertex a
+                    dist = a->w.length();
+                }
+                else if(b_dot_ba < 0)
+                {
+                    // Pick distance vertex b
+                    dist = b->w.length();
+                }
+                else
+                {
+                    // Pick distance to edge a->b
+                    const btScalar a_dot_b = btDot(a->w, b->w);
+                    dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0));
+                }
+                
+                return true;
+            }
+            
+            return false;
+        }
+        sFace*				newface(typename GJK<btConvexTemplate>::sSV* a,typename GJK<btConvexTemplate>::sSV* b,typename GJK<btConvexTemplate>::sSV* c,bool forced)
+        {
+            if(m_stock.root)
+            {
+                sFace*	face=m_stock.root;
+                remove(m_stock,face);
+                append(m_hull,face);
+                face->pass	=	0;
+                face->c[0]	=	a;
+                face->c[1]	=	b;
+                face->c[2]	=	c;
+                face->n		=	btCross(b->w-a->w,c->w-a->w);
+                const btScalar	l=face->n.length();
+                const bool		v=l>EPA_ACCURACY;
+                
+                if(v)
+                {
+                    if(!(getedgedist(face, a, b, face->d) ||
+                         getedgedist(face, b, c, face->d) ||
+                         getedgedist(face, c, a, face->d)))
+                    {
+                        // Origin projects to the interior of the triangle
+                        // Use distance to triangle plane
+                        face->d = btDot(a->w, face->n) / l;
+                    }
+                    
+                    face->n /= l;
+                    if(forced || (face->d >= -EPA_PLANE_EPS))
+                    {
+                        return face;
+                    }
+                    else
+                        m_status=eEpaNonConvex;
+                }
+                else
+                    m_status=eEpaDegenerated;
+                
+                remove(m_hull, face);
+                append(m_stock, face);
+                return 0;
+                
+            }
+            m_status = m_stock.root ? eEpaOutOfVertices : eEpaOutOfFaces;
+            return 0;
+        }
+        sFace*				findbest()
+        {
+            sFace*		minf=m_hull.root;
+            btScalar	mind=minf->d*minf->d;
+            for(sFace* f=minf->l[1];f;f=f->l[1])
+            {
+                const btScalar	sqd=f->d*f->d;
+                if(sqd<mind)
+                {
+                    minf=f;
+                    mind=sqd;
+                }
+            }
+            return(minf);
+        }
+        bool				expand(U pass,typename GJK<btConvexTemplate>::sSV* w,sFace* f,U e,sHorizon& horizon)
+        {
+            static const U	i1m3[]={1,2,0};
+            static const U	i2m3[]={2,0,1};
+            if(f->pass!=pass)
+            {
+                const U	e1=i1m3[e];
+                if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
+                {
+                    sFace*	nf=newface(f->c[e1],f->c[e],w,false);
+                    if(nf)
+                    {
+                        bind(nf,0,f,e);
+                        if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
+                        horizon.cf=nf;
+                        ++horizon.nf;
+                        return(true);
+                    }
+                }
+                else
+                {
+                    const U	e2=i2m3[e];
+                    f->pass		=	(U1)pass;
+                    if(	expand(pass,w,f->f[e1],f->e[e1],horizon)&&
+                       expand(pass,w,f->f[e2],f->e[e2],horizon))
+                    {
+                        remove(m_hull,f);
+                        append(m_stock,f);
+                        return(true);
+                    }
+                }
+            }
+            return(false);
+        }
+        
+    };
+    
+    template <typename btConvexTemplate>
+    static void	Initialize(	const btConvexTemplate& a, const btConvexTemplate& b,
+                           btGjkEpaSolver3::sResults& results,
+                           MinkowskiDiff<btConvexTemplate>& shape)
+    {
+        /* Results		*/ 
+        results.witnesses[0]	=
+        results.witnesses[1]	=	btVector3(0,0,0);
+        results.status			=	btGjkEpaSolver3::sResults::Separated;
+        /* Shape		*/ 
+       
+        shape.m_toshape1		=	b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis());
+        shape.m_toshape0		=	a.getWorldTransform().inverseTimes(b.getWorldTransform());
+        
+    }
+    
+
+//
+// Api
+//
+
+
+
+//
+template <typename btConvexTemplate>
+bool		btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b,
+                                      const btVector3& guess,
+                                      btGjkEpaSolver3::sResults& results)
+{
+    MinkowskiDiff<btConvexTemplate>			shape(a,b);
+    Initialize(a,b,results,shape);
+    GJK<btConvexTemplate>				gjk(a,b);
+    eGjkStatus	gjk_status=gjk.Evaluate(shape,guess);
+    if(gjk_status==eGjkValid)
+    {
+        btVector3	w0=btVector3(0,0,0);
+        btVector3	w1=btVector3(0,0,0);
+        for(U i=0;i<gjk.m_simplex->rank;++i)
+        {
+            const btScalar	p=gjk.m_simplex->p[i];
+            w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
+            w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
+        }
+        results.witnesses[0]	=	a.getWorldTransform()*w0;
+        results.witnesses[1]	=	a.getWorldTransform()*w1;
+        results.normal			=	w0-w1;
+        results.distance		=	results.normal.length();
+        results.normal			/=	results.distance>GJK_MIN_DISTANCE?results.distance:1;
+        return(true);
+    }
+    else
+    {
+        results.status	=	gjk_status==eGjkInside?
+        btGjkEpaSolver3::sResults::Penetrating	:
+        btGjkEpaSolver3::sResults::GJK_Failed	;
+        return(false);
+    }
+}
+
+
+template <typename btConvexTemplate>
+bool	btGjkEpaSolver3_Penetration(const btConvexTemplate& a,
+                                     const btConvexTemplate& b,
+                                     const btVector3& guess,
+                                     btGjkEpaSolver3::sResults& results)
+{
+    MinkowskiDiff<btConvexTemplate>			shape(a,b);
+    Initialize(a,b,results,shape);
+    GJK<btConvexTemplate>				gjk(a,b);
+    eGjkStatus	gjk_status=gjk.Evaluate(shape,-guess);
+    switch(gjk_status)
+    {
+        case	eGjkInside:
+        {
+            EPA<btConvexTemplate>				epa;
+            eEpaStatus	epa_status=epa.Evaluate(gjk,-guess);
+            if(epa_status!=eEpaFailed)
+            {
+                btVector3	w0=btVector3(0,0,0);
+                for(U i=0;i<epa.m_result.rank;++i)
+                {
+                    w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
+                }
+                results.status			=	btGjkEpaSolver3::sResults::Penetrating;
+                results.witnesses[0]	=	a.getWorldTransform()*w0;
+                results.witnesses[1]	=	a.getWorldTransform()*(w0-epa.m_normal*epa.m_depth);
+                results.normal			=	-epa.m_normal;
+                results.distance		=	-epa.m_depth;
+                return(true);
+            } else results.status=btGjkEpaSolver3::sResults::EPA_Failed;
+        }
+            break;
+        case	eGjkFailed:
+            results.status=btGjkEpaSolver3::sResults::GJK_Failed;
+            break;
+        default:
+        {
+        }
+    }
+    return(false);
+}
+
+#if 0
+int	btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanceInfo* distInfo)
+{
+    btGjkEpaSolver3::sResults results;
+    btVector3 guess = colDesc.m_firstDir;
+    
+    bool res = btGjkEpaSolver3::Penetration(colDesc.m_objA,colDesc.m_objB,
+                                            colDesc.m_transformA,colDesc.m_transformB,
+                                            colDesc.m_localSupportFuncA,colDesc.m_localSupportFuncB,
+                                            guess,
+                                            results);
+    if (res)
+    {
+        if ((results.status==btGjkEpaSolver3::sResults::Penetrating) || results.status==GJK::eStatus::Inside)
+        {
+            //normal could be 'swapped'
+            
+            distInfo->m_distance = results.distance;
+            distInfo->m_normalBtoA = results.normal;
+            btVector3 tmpNormalInB = results.witnesses[1]-results.witnesses[0];
+            btScalar lenSqr = tmpNormalInB.length2();
+            if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
+            {
+                tmpNormalInB = results.normal;
+                lenSqr = results.normal.length2();
+            }
+            
+            if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+            {
+                tmpNormalInB /= btSqrt(lenSqr);
+                btScalar distance2 = -(results.witnesses[0]-results.witnesses[1]).length();
+                //only replace valid penetrations when the result is deeper (check)
+                //if ((distance2 < results.distance))
+                {
+                    distInfo->m_distance = distance2;
+                    distInfo->m_pointOnA= results.witnesses[0];
+                    distInfo->m_pointOnB= results.witnesses[1];
+                    distInfo->m_normalBtoA= tmpNormalInB;
+                    return 0;
+                }
+            }
+        }
+        
+    }
+    
+    return -1;
+}
+#endif
+
+template <typename btConvexTemplate, typename btDistanceInfoTemplate>
+int	btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b,
+                         const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo)
+{
+    btGjkEpaSolver3::sResults results;
+    btVector3 guess = colDesc.m_firstDir;
+    
+    bool isSeparated = btGjkEpaSolver3_Distance(	a,b,
+                                                 guess,
+                                                 results);
+    if (isSeparated)
+    {
+        distInfo->m_distance = results.distance;
+        distInfo->m_pointOnA= results.witnesses[0];
+        distInfo->m_pointOnB= results.witnesses[1];
+        distInfo->m_normalBtoA= results.normal;
+        return 0;
+    }
+    
+    return -1;
+}
+
+/* Symbols cleanup		*/ 
+
+#undef GJK_MAX_ITERATIONS
+#undef GJK_ACCURARY
+#undef GJK_MIN_DISTANCE
+#undef GJK_DUPLICATED_EPS
+#undef GJK_SIMPLEX2_EPS
+#undef GJK_SIMPLEX3_EPS
+#undef GJK_SIMPLEX4_EPS
+
+#undef EPA_MAX_VERTICES
+#undef EPA_MAX_FACES
+#undef EPA_MAX_ITERATIONS
+#undef EPA_ACCURACY
+#undef EPA_FALLBACK
+#undef EPA_PLANE_EPS
+#undef EPA_INSIDE_EPS
+
+
+
+#endif //BT_GJK_EPA3_H
+

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

@@ -26,7 +26,6 @@ subject to the following restrictions:
 #ifdef __SPU__
 #include <spu_printf.h>
 #define printf spu_printf
-//#define DEBUG_SPU_COLLISION_DETECTION 1
 #endif //__SPU__
 #endif
 
@@ -81,17 +80,18 @@ void	btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 #ifdef __SPU__
 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
 #else
-void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw)
 #endif
 {
 	m_cachedSeparatingDistance = 0.f;
 
 	btScalar distance=btScalar(0.);
 	btVector3	normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
+
 	btVector3 pointOnA,pointOnB;
 	btTransform	localTransA = input.m_transformA;
 	btTransform localTransB = input.m_transformB;
-	btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
+	btVector3 positionOffset=(localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
 	localTransA.getOrigin() -= positionOffset;
 	localTransB.getOrigin() -= positionOffset;
 
@@ -102,17 +102,11 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 
 	gNumGjkChecks++;
 
-#ifdef DEBUG_SPU_COLLISION_DETECTION
-	spu_printf("inside gjk\n");
-#endif
 	//for CCD we don't use margins
 	if (m_ignoreMargin)
 	{
 		marginA = btScalar(0.);
 		marginB = btScalar(0.);
-#ifdef DEBUG_SPU_COLLISION_DETECTION
-		spu_printf("ignoring margin\n");
-#endif
 	}
 
 	m_curIter = 0;
@@ -143,37 +137,13 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 			btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
 			btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
 
-#if 1
 
 			btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 			btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
 
-//			btVector3 pInA  = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
-//			btVector3 qInB  = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);
-
-#else
-#ifdef __SPU__
-			btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
-			btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
-#else
-			btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
-			btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
-#ifdef TEST_NON_VIRTUAL
-			btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
-			btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
-			btAssert((pInAv-pInA).length() < 0.0001);
-			btAssert((qInBv-qInB).length() < 0.0001);
-#endif //
-#endif //__SPU__
-#endif
-
-
 			btVector3  pWorld = localTransA(pInA);	
 			btVector3  qWorld = localTransB(qInB);
 
-#ifdef DEBUG_SPU_COLLISION_DETECTION
-		spu_printf("got local supporting vertices\n");
-#endif
 
 			if (check2d)
 			{
@@ -217,14 +187,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 				break;
 			}
 
-#ifdef DEBUG_SPU_COLLISION_DETECTION
-		spu_printf("addVertex 1\n");
-#endif
 			//add current vertex to simplex
 			m_simplexSolver->addVertex(w, pWorld, qWorld);
-#ifdef DEBUG_SPU_COLLISION_DETECTION
-		spu_printf("addVertex 2\n");
-#endif
 			btVector3 newCachedSeparatingAxis;
 
 			//calculate the closest point to the origin (update vector v)
@@ -274,7 +238,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 			  //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
               if (m_curIter++ > gGjkMaxIter)   
               {   
-                      #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
+                      #if defined(DEBUG) || defined (_DEBUG)
 
                               printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);   
                               printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
@@ -307,6 +271,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 		{
 			m_simplexSolver->compute_points(pointOnA, pointOnB);
 			normalInB = m_cachedSeparatingAxis;
+
 			btScalar lenSqr =m_cachedSeparatingAxis.length2();
 			
 			//valid normal
@@ -318,6 +283,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 			{
 				btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
 				normalInB *= rlen; //normalize
+
 				btScalar s = btSqrt(squaredDistance);
 			
 				btAssert(s > btScalar(0.0));
@@ -380,6 +346,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 							pointOnA = tmpPointOnA;
 							pointOnB = tmpPointOnB;
 							normalInB = tmpNormalInB;
+
 							isValid = true;
 							m_lastUsedMethod = 3;
 						} else
@@ -413,6 +380,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 							pointOnB += m_cachedSeparatingAxis * marginB ;
 							normalInB = m_cachedSeparatingAxis;
 							normalInB.normalize();
+
 							isValid = true;
 							m_lastUsedMethod = 6;
 						} else
@@ -431,36 +399,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 
 	if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
 	{
-#if 0
-///some debugging
-//		if (check2d)
-		{
-			printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]);
-			printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex);
-		}
-#endif 
 
-		if (m_fixContactNormalDirection)
-		{
-			///@workaround for sticky convex collisions
-			//in some degenerate cases (usually when the use uses very small margins) 
-			//the contact normal is pointing the wrong direction
-			//so fix it now (until we can deal with all degenerate cases in GJK and EPA)
-			//contact normals need to point from B to A in all cases, so we can simply check if the contact normal really points from B to A
-			//We like to use a dot product of the normal against the difference of the centroids, 
-			//once the centroid is available in the API
-			//until then we use the center of the aabb to approximate the centroid
-			btVector3 aabbMin,aabbMax;
-			m_minkowskiA->getAabb(localTransA,aabbMin,aabbMax);
-			btVector3 posA  = (aabbMax+aabbMin)*btScalar(0.5);
-		
-			m_minkowskiB->getAabb(localTransB,aabbMin,aabbMax);
-			btVector3 posB = (aabbMin+aabbMax)*btScalar(0.5);
-
-			btVector3 diff = posA-posB;
-			if (diff.dot(normalInB) < 0.f)
-				normalInB *= -1.f;
-		}
 		m_cachedSeparatingAxis = normalInB;
 		m_cachedSeparatingDistance = distance;
 

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

@@ -0,0 +1,901 @@
+
+/***
+ * ---------------------------------
+ * Copyright (c)2012 Daniel Fiser <[email protected]>
+ *
+ *  This file was ported from mpr.c file, part of libccd.
+ *  The Minkoski Portal Refinement implementation was ported 
+ *  to OpenCL by Erwin Coumans for the Bullet 3 Physics library.
+ *  The original MPR idea and implementation is by Gary Snethen
+ *  in XenoCollide, see http://github.com/erwincoumans/xenocollide
+ *
+ *  Distributed under the OSI-approved BSD License (the "License");
+ *  see <http://www.opensource.org/licenses/bsd-license.php>.
+ *  This software is distributed WITHOUT ANY WARRANTY; without even the
+ *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *  See the License for more information.
+ */
+
+///2014 Oct, Erwin Coumans, Use templates to avoid void* casts
+
+#ifndef BT_MPR_PENETRATION_H
+#define BT_MPR_PENETRATION_H
+
+#define BT_DEBUG_MPR1
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+//#define MPR_AVERAGE_CONTACT_POSITIONS
+
+
+struct btMprCollisionDescription
+{
+    btVector3	m_firstDir;
+    int			m_maxGjkIterations;
+    btScalar	m_maximumDistanceSquared;
+    btScalar	m_gjkRelError2;
+   
+    btMprCollisionDescription()
+    :	m_firstDir(0,1,0),
+        m_maxGjkIterations(1000),
+        m_maximumDistanceSquared(1e30f),
+        m_gjkRelError2(1.0e-6)
+    {
+    }
+    virtual ~btMprCollisionDescription()
+    {
+    }
+};
+
+struct btMprDistanceInfo
+{
+    btVector3	m_pointOnA;
+    btVector3	m_pointOnB;
+    btVector3	m_normalBtoA;
+    btScalar	m_distance;
+};
+
+#ifdef __cplusplus
+#define BT_MPR_SQRT sqrtf
+#else
+#define BT_MPR_SQRT sqrt
+#endif
+#define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
+#define BT_MPR_FABS fabs
+
+#define BT_MPR_TOLERANCE 1E-6f
+#define BT_MPR_MAX_ITERATIONS 1000
+
+struct _btMprSupport_t 
+{
+    btVector3 v;  //!< Support point in minkowski sum
+    btVector3 v1; //!< Support point in obj1
+    btVector3 v2; //!< Support point in obj2
+};
+typedef struct _btMprSupport_t btMprSupport_t;
+
+struct _btMprSimplex_t 
+{
+    btMprSupport_t ps[4];
+    int last; //!< index of last added point
+};
+typedef struct _btMprSimplex_t btMprSimplex_t;
+
+inline btMprSupport_t* btMprSimplexPointW(btMprSimplex_t *s, int idx)
+{
+    return &s->ps[idx];
+}
+
+inline void btMprSimplexSetSize(btMprSimplex_t *s, int size)
+{
+    s->last = size - 1;
+}
+
+#ifdef DEBUG_MPR
+inline void btPrintPortalVertex(_btMprSimplex_t* portal, int index)
+{
+    printf("portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->ps[index].v.x(),portal->ps[index].v.y(),portal->ps[index].v.z(),
+           portal->ps[index].v1.x(),portal->ps[index].v1.y(),portal->ps[index].v1.z(),
+           portal->ps[index].v2.x(),portal->ps[index].v2.y(),portal->ps[index].v2.z());
+}
+#endif //DEBUG_MPR
+
+
+
+
+inline int btMprSimplexSize(const btMprSimplex_t *s)
+{
+    return s->last + 1;
+}
+
+
+inline const btMprSupport_t* btMprSimplexPoint(const btMprSimplex_t* s, int idx)
+{
+    // here is no check on boundaries
+    return &s->ps[idx];
+}
+
+inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
+{
+    *d = *s;
+}
+
+inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
+{
+    btMprSupportCopy(s->ps + pos, a);
+}
+
+
+inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
+{
+    btMprSupport_t supp;
+
+    btMprSupportCopy(&supp, &s->ps[pos1]);
+    btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
+    btMprSupportCopy(&s->ps[pos2], &supp);
+}
+
+
+inline int btMprIsZero(float val)
+{
+    return BT_MPR_FABS(val) < FLT_EPSILON;
+}
+
+
+
+inline int btMprEq(float _a, float _b)
+{
+    float ab;
+    float a, b;
+
+    ab = BT_MPR_FABS(_a - _b);
+    if (BT_MPR_FABS(ab) < FLT_EPSILON)
+        return 1;
+
+    a = BT_MPR_FABS(_a);
+    b = BT_MPR_FABS(_b);
+    if (b > a){
+        return ab < FLT_EPSILON * b;
+    }else{
+        return ab < FLT_EPSILON * a;
+    }
+}
+
+
+inline int btMprVec3Eq(const btVector3* a, const btVector3 *b)
+{
+    return btMprEq((*a).x(), (*b).x())
+            && btMprEq((*a).y(), (*b).y())
+            && btMprEq((*a).z(), (*b).z());
+}
+
+
+
+
+
+
+
+
+
+
+
+template <typename btConvexTemplate>
+inline void btFindOrigin(const btConvexTemplate& a, const btConvexTemplate& b, const btMprCollisionDescription& colDesc,btMprSupport_t *center)
+{
+
+	center->v1 = a.getObjectCenterInWorld();
+    center->v2 = b.getObjectCenterInWorld();
+    center->v = center->v1 - center->v2;
+}
+
+inline void btMprVec3Set(btVector3 *v, float x, float y, float z)
+{
+	v->setValue(x,y,z);
+}
+
+inline void btMprVec3Add(btVector3 *v, const btVector3 *w)
+{
+	*v += *w;
+}
+
+inline void btMprVec3Copy(btVector3 *v, const btVector3 *w)
+{
+    *v = *w;
+}
+
+inline void btMprVec3Scale(btVector3 *d, float k)
+{
+    *d *= k;
+}
+
+inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
+{
+    float dot;
+
+	dot = btDot(*a,*b);
+    return dot;
+}
+
+
+inline float btMprVec3Len2(const btVector3 *v)
+{
+    return btMprVec3Dot(v, v);
+}
+
+inline void btMprVec3Normalize(btVector3 *d)
+{
+    float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d));
+    btMprVec3Scale(d, k);
+}
+
+inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
+{
+	*d = btCross(*a,*b);
+	
+}
+
+
+inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
+{
+	*d = *v - *w;
+}
+
+inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir)
+{
+    btVector3 v2v1, v3v1;
+
+    btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v,
+                       &btMprSimplexPoint(portal, 1)->v);
+    btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v,
+                       &btMprSimplexPoint(portal, 1)->v);
+    btMprVec3Cross(dir, &v2v1, &v3v1);
+    btMprVec3Normalize(dir);
+}
+
+
+inline int portalEncapsulesOrigin(const btMprSimplex_t *portal,
+                                       const btVector3 *dir)
+{
+    float dot;
+    dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v);
+    return btMprIsZero(dot) || dot > 0.f;
+}
+
+inline int portalReachTolerance(const btMprSimplex_t *portal,
+                                     const btMprSupport_t *v4,
+                                     const btVector3 *dir)
+{
+    float dv1, dv2, dv3, dv4;
+    float dot1, dot2, dot3;
+
+    // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
+
+    dv1 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir);
+    dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir);
+    dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir);
+    dv4 = btMprVec3Dot(&v4->v, dir);
+
+    dot1 = dv4 - dv1;
+    dot2 = dv4 - dv2;
+    dot3 = dv4 - dv3;
+
+    dot1 = BT_MPR_FMIN(dot1, dot2);
+    dot1 = BT_MPR_FMIN(dot1, dot3);
+
+    return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE;
+}
+
+inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal,
+                                         const btMprSupport_t *v4,
+                                         const btVector3 *dir)
+{
+    float dot;
+    dot = btMprVec3Dot(&v4->v, dir);
+    return btMprIsZero(dot) || dot > 0.f;
+}
+
+inline void btExpandPortal(btMprSimplex_t *portal,
+                              const btMprSupport_t *v4)
+{
+    float dot;
+    btVector3 v4v0;
+
+    btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v);
+    dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0);
+    if (dot > 0.f){
+        dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0);
+        if (dot > 0.f){
+            btMprSimplexSet(portal, 1, v4);
+        }else{
+            btMprSimplexSet(portal, 3, v4);
+        }
+    }else{
+        dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0);
+        if (dot > 0.f){
+            btMprSimplexSet(portal, 2, v4);
+        }else{
+            btMprSimplexSet(portal, 1, v4);
+        }
+    }
+}
+template <typename btConvexTemplate>
+inline void btMprSupport(const btConvexTemplate& a, const btConvexTemplate& b,
+                         const btMprCollisionDescription& colDesc,
+													const btVector3& dir, btMprSupport_t *supp)
+{
+	btVector3 seperatingAxisInA = dir* a.getWorldTransform().getBasis();
+	btVector3 seperatingAxisInB = -dir* b.getWorldTransform().getBasis();
+
+	btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA);
+	btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB);
+
+	supp->v1 = a.getWorldTransform()(pInA);
+	supp->v2 = b.getWorldTransform()(qInB);
+	supp->v = supp->v1 - supp->v2;
+}
+
+
+template <typename btConvexTemplate>
+static int btDiscoverPortal(const btConvexTemplate& a, const btConvexTemplate& b,
+                            const btMprCollisionDescription& colDesc,
+													btMprSimplex_t *portal)
+{
+    btVector3 dir, va, vb;
+    float dot;
+    int cont;
+	
+	
+
+    // vertex 0 is center of portal
+    btFindOrigin(a,b,colDesc, btMprSimplexPointW(portal, 0));
+   
+    
+    // vertex 0 is center of portal
+    btMprSimplexSetSize(portal, 1);
+    
+
+
+	btVector3 zero = btVector3(0,0,0);
+	btVector3* org = &zero;
+
+    if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org)){
+        // Portal's center lies on origin (0,0,0) => we know that objects
+        // intersect but we would need to know penetration info.
+        // So move center little bit...
+        btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
+        btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va);
+    }
+
+
+    // vertex 1 = support in direction of origin
+    btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v);
+    btMprVec3Scale(&dir, -1.f);
+    btMprVec3Normalize(&dir);
+
+
+    btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 1));
+ 
+    btMprSimplexSetSize(portal, 2);
+
+    // test if origin isn't outside of v1
+    dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir);
+	
+
+    if (btMprIsZero(dot) || dot < 0.f)
+        return -1;
+
+
+    // vertex 2
+    btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v,
+                       &btMprSimplexPoint(portal, 1)->v);
+    if (btMprIsZero(btMprVec3Len2(&dir))){
+        if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org)){
+            // origin lies on v1
+            return 1;
+        }else{
+            // origin lies on v0-v1 segment
+            return 2;
+        }
+    }
+
+    btMprVec3Normalize(&dir);
+    btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 2));
+    
+    
+    
+    dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir);
+    if (btMprIsZero(dot) || dot < 0.f)
+        return -1;
+
+    btMprSimplexSetSize(portal, 3);
+
+    // vertex 3 direction
+    btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
+                     &btMprSimplexPoint(portal, 0)->v);
+    btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
+                     &btMprSimplexPoint(portal, 0)->v);
+    btMprVec3Cross(&dir, &va, &vb);
+    btMprVec3Normalize(&dir);
+
+    // it is better to form portal faces to be oriented "outside" origin
+    dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v);
+    if (dot > 0.f){
+        btMprSimplexSwap(portal, 1, 2);
+        btMprVec3Scale(&dir, -1.f);
+    }
+
+    while (btMprSimplexSize(portal) < 4){
+		 btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 3));
+        
+        dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir);
+        if (btMprIsZero(dot) || dot < 0.f)
+            return -1;
+
+        cont = 0;
+
+        // test if origin is outside (v1, v0, v3) - set v2 as v3 and
+        // continue
+        btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v,
+                          &btMprSimplexPoint(portal, 3)->v);
+        dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
+        if (dot < 0.f && !btMprIsZero(dot)){
+            btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3));
+            cont = 1;
+        }
+
+        if (!cont){
+            // test if origin is outside (v3, v0, v2) - set v1 as v3 and
+            // continue
+            btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v,
+                              &btMprSimplexPoint(portal, 2)->v);
+            dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
+            if (dot < 0.f && !btMprIsZero(dot)){
+                btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3));
+                cont = 1;
+            }
+        }
+
+        if (cont){
+            btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
+                             &btMprSimplexPoint(portal, 0)->v);
+            btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
+                             &btMprSimplexPoint(portal, 0)->v);
+            btMprVec3Cross(&dir, &va, &vb);
+            btMprVec3Normalize(&dir);
+        }else{
+            btMprSimplexSetSize(portal, 4);
+        }
+    }
+
+    return 0;
+}
+
+template <typename btConvexTemplate>
+static int btRefinePortal(const btConvexTemplate& a, const btConvexTemplate& b,const btMprCollisionDescription& colDesc,
+							btMprSimplex_t *portal)
+{
+    btVector3 dir;
+    btMprSupport_t v4;
+
+	for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++)
+    //while (1)
+	{
+        // compute direction outside the portal (from v0 throught v1,v2,v3
+        // face)
+        btPortalDir(portal, &dir);
+
+        // test if origin is inside the portal
+        if (portalEncapsulesOrigin(portal, &dir))
+            return 0;
+
+        // get next support point
+        
+		 btMprSupport(a,b,colDesc, dir, &v4);
+
+
+        // test if v4 can expand portal to contain origin and if portal
+        // expanding doesn't reach given tolerance
+        if (!portalCanEncapsuleOrigin(portal, &v4, &dir)
+                || portalReachTolerance(portal, &v4, &dir))
+		{
+            return -1;
+        }
+
+        // v1-v2-v3 triangle must be rearranged to face outside Minkowski
+        // difference (direction from v0).
+        btExpandPortal(portal, &v4);
+    }
+
+    return -1;
+}
+
+static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos)
+{
+
+	btVector3 zero = btVector3(0,0,0);
+	btVector3* origin = &zero;
+
+    btVector3 dir;
+    size_t i;
+    float b[4], sum, inv;
+    btVector3 vec, p1, p2;
+
+    btPortalDir(portal, &dir);
+
+    // use barycentric coordinates of tetrahedron to find origin
+    btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
+                       &btMprSimplexPoint(portal, 2)->v);
+    b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
+
+    btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
+                       &btMprSimplexPoint(portal, 2)->v);
+    b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
+
+    btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v,
+                       &btMprSimplexPoint(portal, 1)->v);
+    b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
+
+    btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
+                       &btMprSimplexPoint(portal, 1)->v);
+    b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
+
+	sum = b[0] + b[1] + b[2] + b[3];
+
+    if (btMprIsZero(sum) || sum < 0.f){
+		b[0] = 0.f;
+
+        btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
+                           &btMprSimplexPoint(portal, 3)->v);
+        b[1] = btMprVec3Dot(&vec, &dir);
+        btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
+                           &btMprSimplexPoint(portal, 1)->v);
+        b[2] = btMprVec3Dot(&vec, &dir);
+        btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
+                           &btMprSimplexPoint(portal, 2)->v);
+        b[3] = btMprVec3Dot(&vec, &dir);
+
+		sum = b[1] + b[2] + b[3];
+	}
+
+	inv = 1.f / sum;
+
+    btMprVec3Copy(&p1, origin);
+    btMprVec3Copy(&p2, origin);
+    for (i = 0; i < 4; i++){
+        btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1);
+        btMprVec3Scale(&vec, b[i]);
+        btMprVec3Add(&p1, &vec);
+
+        btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2);
+        btMprVec3Scale(&vec, b[i]);
+        btMprVec3Add(&p2, &vec);
+    }
+    btMprVec3Scale(&p1, inv);
+    btMprVec3Scale(&p2, inv);
+#ifdef MPR_AVERAGE_CONTACT_POSITIONS
+    btMprVec3Copy(pos, &p1);
+    btMprVec3Add(pos, &p2);
+    btMprVec3Scale(pos, 0.5);
+#else
+    btMprVec3Copy(pos, &p2);
+#endif//MPR_AVERAGE_CONTACT_POSITIONS
+}
+
+inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b)
+{
+    btVector3 ab;
+    btMprVec3Sub2(&ab, a, b);
+    return btMprVec3Len2(&ab);
+}
+
+inline float _btMprVec3PointSegmentDist2(const btVector3 *P,
+                                                  const btVector3 *x0,
+                                                  const btVector3 *b,
+                                                  btVector3 *witness)
+{
+    // The computation comes from solving equation of segment:
+    //      S(t) = x0 + t.d
+    //          where - x0 is initial point of segment
+    //                - d is direction of segment from x0 (|d| > 0)
+    //                - t belongs to <0, 1> interval
+    // 
+    // Than, distance from a segment to some point P can be expressed:
+    //      D(t) = |x0 + t.d - P|^2
+    //          which is distance from any point on segment. Minimization
+    //          of this function brings distance from P to segment.
+    // Minimization of D(t) leads to simple quadratic equation that's
+    // solving is straightforward.
+    //
+    // Bonus of this method is witness point for free.
+
+    float dist, t;
+    btVector3 d, a;
+
+    // direction of segment
+    btMprVec3Sub2(&d, b, x0);
+
+    // precompute vector from P to x0
+    btMprVec3Sub2(&a, x0, P);
+
+    t  = -1.f * btMprVec3Dot(&a, &d);
+    t /= btMprVec3Len2(&d);
+
+    if (t < 0.f || btMprIsZero(t)){
+        dist = btMprVec3Dist2(x0, P);
+        if (witness)
+            btMprVec3Copy(witness, x0);
+    }else if (t > 1.f || btMprEq(t, 1.f)){
+        dist = btMprVec3Dist2(b, P);
+        if (witness)
+            btMprVec3Copy(witness, b);
+    }else{
+        if (witness){
+            btMprVec3Copy(witness, &d);
+            btMprVec3Scale(witness, t);
+            btMprVec3Add(witness, x0);
+            dist = btMprVec3Dist2(witness, P);
+        }else{
+            // recycling variables
+            btMprVec3Scale(&d, t);
+            btMprVec3Add(&d, &a);
+            dist = btMprVec3Len2(&d);
+        }
+    }
+
+    return dist;
+}
+
+
+
+inline float btMprVec3PointTriDist2(const btVector3 *P,
+                                const btVector3 *x0, const btVector3 *B,
+                                const btVector3 *C,
+                                btVector3 *witness)
+{
+    // Computation comes from analytic expression for triangle (x0, B, C)
+    //      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
+    // Then equation for distance is:
+    //      D(s, t) = | T(s, t) - P |^2
+    // This leads to minimization of quadratic function of two variables.
+    // The solution from is taken only if s is between 0 and 1, t is
+    // between 0 and 1 and t + s < 1, otherwise distance from segment is
+    // computed.
+
+    btVector3 d1, d2, a;
+    float u, v, w, p, q, r;
+    float s, t, dist, dist2;
+    btVector3 witness2;
+
+    btMprVec3Sub2(&d1, B, x0);
+    btMprVec3Sub2(&d2, C, x0);
+    btMprVec3Sub2(&a, x0, P);
+
+    u = btMprVec3Dot(&a, &a);
+    v = btMprVec3Dot(&d1, &d1);
+    w = btMprVec3Dot(&d2, &d2);
+    p = btMprVec3Dot(&a, &d1);
+    q = btMprVec3Dot(&a, &d2);
+    r = btMprVec3Dot(&d1, &d2);
+
+    s = (q * r - w * p) / (w * v - r * r);
+    t = (-s * r - q) / w;
+
+    if ((btMprIsZero(s) || s > 0.f)
+            && (btMprEq(s, 1.f) || s < 1.f)
+            && (btMprIsZero(t) || t > 0.f)
+            && (btMprEq(t, 1.f) || t < 1.f)
+            && (btMprEq(t + s, 1.f) || t + s < 1.f)){
+
+        if (witness){
+            btMprVec3Scale(&d1, s);
+            btMprVec3Scale(&d2, t);
+            btMprVec3Copy(witness, x0);
+            btMprVec3Add(witness, &d1);
+            btMprVec3Add(witness, &d2);
+
+            dist = btMprVec3Dist2(witness, P);
+        }else{
+            dist  = s * s * v;
+            dist += t * t * w;
+            dist += 2.f * s * t * r;
+            dist += 2.f * s * p;
+            dist += 2.f * t * q;
+            dist += u;
+        }
+    }else{
+        dist = _btMprVec3PointSegmentDist2(P, x0, B, witness);
+
+        dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2);
+        if (dist2 < dist){
+            dist = dist2;
+            if (witness)
+                btMprVec3Copy(witness, &witness2);
+        }
+
+        dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2);
+        if (dist2 < dist){
+            dist = dist2;
+            if (witness)
+                btMprVec3Copy(witness, &witness2);
+        }
+    }
+
+    return dist;
+}
+
+template <typename btConvexTemplate>
+static void btFindPenetr(const btConvexTemplate& a, const btConvexTemplate& b,
+                         const btMprCollisionDescription& colDesc,
+                         btMprSimplex_t *portal,
+                         float *depth, btVector3 *pdir, btVector3 *pos)
+{
+    btVector3 dir;
+    btMprSupport_t v4;
+    unsigned long iterations;
+
+	btVector3 zero = btVector3(0,0,0);
+	btVector3* origin = &zero;
+
+
+    iterations = 1UL;
+	for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++)
+    //while (1)
+	{
+        // compute portal direction and obtain next support point
+        btPortalDir(portal, &dir);
+        
+		 btMprSupport(a,b,colDesc, dir, &v4);
+
+
+        // reached tolerance -> find penetration info
+        if (portalReachTolerance(portal, &v4, &dir)
+                || iterations ==BT_MPR_MAX_ITERATIONS)
+		{
+            *depth = btMprVec3PointTriDist2(origin,&btMprSimplexPoint(portal, 1)->v,&btMprSimplexPoint(portal, 2)->v,&btMprSimplexPoint(portal, 3)->v,pdir);
+            *depth = BT_MPR_SQRT(*depth);
+			
+			if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z()))
+			{
+				
+				*pdir = dir;
+			} 
+			btMprVec3Normalize(pdir);
+			
+            // barycentric coordinates:
+            btFindPos(portal, pos);
+
+
+            return;
+        }
+
+        btExpandPortal(portal, &v4);
+
+        iterations++;
+    }
+}
+
+static void btFindPenetrTouch(btMprSimplex_t *portal,float *depth, btVector3 *dir, btVector3 *pos)
+{
+    // Touching contact on portal's v1 - so depth is zero and direction
+    // is unimportant and pos can be guessed
+    *depth = 0.f;
+    btVector3 zero = btVector3(0,0,0);
+	btVector3* origin = &zero;
+
+
+	btMprVec3Copy(dir, origin);
+#ifdef MPR_AVERAGE_CONTACT_POSITIONS
+    btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
+    btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
+    btMprVec3Scale(pos, 0.5);
+#else
+     btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
+#endif
+}
+
+static void btFindPenetrSegment(btMprSimplex_t *portal,
+                              float *depth, btVector3 *dir, btVector3 *pos)
+{
+    
+    // Origin lies on v0-v1 segment.
+    // Depth is distance to v1, direction also and position must be
+    // computed
+#ifdef MPR_AVERAGE_CONTACT_POSITIONS
+    btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
+    btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
+    btMprVec3Scale(pos, 0.5f);
+#else
+     btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
+#endif//MPR_AVERAGE_CONTACT_POSITIONS
+
+    btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v);
+    *depth = BT_MPR_SQRT(btMprVec3Len2(dir));
+    btMprVec3Normalize(dir);
+
+    
+}
+
+
+template <typename btConvexTemplate>
+inline int btMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b,
+                            const btMprCollisionDescription& colDesc,
+					float *depthOut, btVector3* dirOut, btVector3* posOut)
+{
+	
+	 btMprSimplex_t portal;
+
+
+    // Phase 1: Portal discovery
+    int result = btDiscoverPortal(a,b,colDesc, &portal);
+	
+	  
+	//sepAxis[pairIndex] = *pdir;//or -dir?
+
+	switch (result)
+	{
+	case 0:
+		{
+			// Phase 2: Portal refinement
+		
+			result = btRefinePortal(a,b,colDesc, &portal);
+			if (result < 0)
+				return -1;
+
+			// Phase 3. Penetration info
+			btFindPenetr(a,b,colDesc, &portal, depthOut, dirOut, posOut);
+			
+			
+			break;
+		}
+	case 1:
+		{
+			 // Touching contact on portal's v1.
+			btFindPenetrTouch(&portal, depthOut, dirOut, posOut);
+			result=0;
+			break;
+		}
+	case 2:
+		{
+			
+			btFindPenetrSegment( &portal, depthOut, dirOut, posOut);
+			result=0;
+			break;
+		}
+	default:
+		{
+			//if (res < 0)
+			//{
+				// Origin isn't inside portal - no collision.
+				result = -1;
+			//}
+		}
+	};
+	
+	return result;
+};
+
+
+template<typename btConvexTemplate, typename btMprDistanceTemplate>
+inline int	btComputeMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, const
+                                    btMprCollisionDescription& colDesc, btMprDistanceTemplate* distInfo)
+{
+	btVector3 dir,pos;
+	float depth;
+
+	int res = btMprPenetration(a,b,colDesc,&depth, &dir, &pos);
+	if (res==0)
+	{
+		distInfo->m_distance = -depth;
+		distInfo->m_pointOnB = pos;
+		distInfo->m_normalBtoA = -dir;
+		distInfo->m_pointOnA = pos-distInfo->m_distance*dir;
+		return 0;
+	}
+
+	return -1;
+}
+
+
+
+#endif //BT_MPR_PENETRATION_H

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp

@@ -178,62 +178,62 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans
 }
 #endif //TEST_INTERNAL_OBJECTS
 
- 
- 
- SIMD_FORCE_INLINE void btSegmentsClosestPoints(
-	btVector3& ptsVector,
-	btVector3& offsetA,
-	btVector3& offsetB,
-	btScalar& tA, btScalar& tB,
-	const btVector3& translation,
-	const btVector3& dirA, btScalar hlenA,
-	const btVector3& dirB, btScalar hlenB )
-{
-	// compute the parameters of the closest points on each line segment
-
-	btScalar dirA_dot_dirB = btDot(dirA,dirB);
-	btScalar dirA_dot_trans = btDot(dirA,translation);
-	btScalar dirB_dot_trans = btDot(dirB,translation);
-
-	btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
-
-	if ( denom == 0.0f ) {
-		tA = 0.0f;
-	} else {
-		tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
-		if ( tA < -hlenA )
-			tA = -hlenA;
-		else if ( tA > hlenA )
-			tA = hlenA;
-	}
-
-	tB = tA * dirA_dot_dirB - dirB_dot_trans;
-
-	if ( tB < -hlenB ) {
-		tB = -hlenB;
-		tA = tB * dirA_dot_dirB + dirA_dot_trans;
-
-		if ( tA < -hlenA )
-			tA = -hlenA;
-		else if ( tA > hlenA )
-			tA = hlenA;
-	} else if ( tB > hlenB ) {
-		tB = hlenB;
-		tA = tB * dirA_dot_dirB + dirA_dot_trans;
-
-		if ( tA < -hlenA )
-			tA = -hlenA;
-		else if ( tA > hlenA )
-			tA = hlenA;
-	}
-
-	// compute the closest points relative to segment centers.
-
-	offsetA = dirA * tA;
-	offsetB = dirB * tB;
-
-	ptsVector = translation - offsetA + offsetB;
-}
+ 
+ 
+ SIMD_FORCE_INLINE void btSegmentsClosestPoints(
+	btVector3& ptsVector,
+	btVector3& offsetA,
+	btVector3& offsetB,
+	btScalar& tA, btScalar& tB,
+	const btVector3& translation,
+	const btVector3& dirA, btScalar hlenA,
+	const btVector3& dirB, btScalar hlenB )
+{
+	// compute the parameters of the closest points on each line segment
+
+	btScalar dirA_dot_dirB = btDot(dirA,dirB);
+	btScalar dirA_dot_trans = btDot(dirA,translation);
+	btScalar dirB_dot_trans = btDot(dirB,translation);
+
+	btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
+
+	if ( denom == 0.0f ) {
+		tA = 0.0f;
+	} else {
+		tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	}
+
+	tB = tA * dirA_dot_dirB - dirB_dot_trans;
+
+	if ( tB < -hlenB ) {
+		tB = -hlenB;
+		tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	} else if ( tB > hlenB ) {
+		tB = hlenB;
+		tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	}
+
+	// compute the closest points relative to segment centers.
+
+	offsetA = dirA * tA;
+	offsetB = dirB * tB;
+
+	ptsVector = translation - offsetA + offsetB;
+}
 
 
 
@@ -313,7 +313,7 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 	int edgeB=-1;
 	btVector3 worldEdgeA;
 	btVector3 worldEdgeB;
-	btVector3 witnessPointA,witnessPointB;
+	btVector3 witnessPointA(0,0,0),witnessPointB(0,0,0);
 	
 
 	int curEdgeEdge = 0;
@@ -369,23 +369,23 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 //		printf("edge-edge\n");
 		//add an edge-edge contact
 
-		btVector3 ptsVector;
-		btVector3 offsetA;
-		btVector3 offsetB;
-		btScalar tA;
-		btScalar tB;
-
-		btVector3 translation = witnessPointB-witnessPointA;
-
-		btVector3 dirA = worldEdgeA;
-		btVector3 dirB = worldEdgeB;
-		
-		btScalar hlenB = 1e30f;
-		btScalar hlenA = 1e30f;
-
-		btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB,
-			translation,
-			dirA, hlenA,
+		btVector3 ptsVector;
+		btVector3 offsetA;
+		btVector3 offsetB;
+		btScalar tA;
+		btScalar tB;
+
+		btVector3 translation = witnessPointB-witnessPointA;
+
+		btVector3 dirA = worldEdgeA;
+		btVector3 dirB = worldEdgeB;
+		
+		btScalar hlenB = 1e30f;
+		btScalar hlenA = 1e30f;
+
+		btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB,
+			translation,
+			dirA, hlenA,
 			dirB,hlenB);
 
 		btScalar nlSqrt = ptsVector.length2();

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

@@ -32,10 +32,12 @@ public:
    //@BP Mod - allow backface filtering and unflipped normals
    enum EFlags
    {
-      kF_None                 = 0,
+	  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
+		///SubSimplexConvexCastRaytest is the default, even if kF_None is set.
+	  kF_UseSubSimplexConvexCastRaytest = 1 << 2,   // Uses an approximate but faster ray versus convex intersection algorithm
+	  kF_UseGjkConvexCastRaytest = 1 << 3,
       kF_Terminator        = 0xFFFFFFFF
    };
    unsigned int m_flags;

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp

@@ -65,10 +65,10 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 
 	btVector3 n;
 	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
-	bool hasResult = false;
+	
 	btVector3 c;
 
-	btScalar lastLambda = lambda;
+	
 
 
 	btScalar dist2 = v.length2();
@@ -109,9 +109,9 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 				//m_simplexSolver->reset();
 				//check next line
 				 w = supVertexA-supVertexB;
-				lastLambda = lambda;
+				
 				n = v;
-				hasResult = true;
+				
 			}
 		} 
 		///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc.
@@ -121,7 +121,7 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 		if (m_simplexSolver->closest(v))
 		{
 			dist2 = v.length2();
-			hasResult = true;
+			
 			//todo: check this normal for validity
 			//n=v;
 			//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);

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

@@ -29,9 +29,10 @@ subject to the following restrictions:
 static btVector3
 getNormalizedVector(const btVector3& v)
 {
-	btVector3 n = v.normalized();
-	if (n.length() < SIMD_EPSILON) {
-		n.setValue(0, 0, 0);
+	btVector3 n(0, 0, 0);
+
+	if (v.length() > SIMD_EPSILON) {
+		n = v.normalized();
 	}
 	return n;
 }
@@ -380,8 +381,8 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 		if (callback.hasHit())
 		{	
 			// we moved only a fraction
-			btScalar hitDistance;
-			hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
+			//btScalar hitDistance;
+			//hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
 
 //			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 
@@ -635,7 +636,7 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 //	printf("  dt = %f", dt);
 
 	// quick check...
-	if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
+	if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) {
 //		printf("\n");
 		return;		// no motion
 	}

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

@@ -540,8 +540,8 @@ void btConeTwistConstraint::calcAngleInfo()
 	m_solveTwistLimit = false;
 	m_solveSwingLimit = false;
 
-	btVector3 b1Axis1,b1Axis2,b1Axis3;
-	btVector3 b2Axis1,b2Axis2;
+	btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
+	btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0);
 
 	b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
 	b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
@@ -983,8 +983,8 @@ void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingA
 
 void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
 {
-	btTransform trACur = m_rbA.getCenterOfMassTransform();
-	btTransform trBCur = m_rbB.getCenterOfMassTransform();
+	//btTransform trACur = m_rbA.getCenterOfMassTransform();
+	//btTransform trBCur = m_rbB.getCenterOfMassTransform();
 //	btTransform trABCur = trBCur.inverse() * trACur;
 //	btQuaternion qABCur = trABCur.getRotation();
 //	btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);

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

@@ -33,7 +33,8 @@ class	btDispatcher;
 enum btConstraintSolverType
 {
 	BT_SEQUENTIAL_IMPULSE_SOLVER=1,
-	BT_MLCP_SOLVER=2
+	BT_MLCP_SOLVER=2,
+	BT_NNCG_SOLVER=4
 };
 
 class btConstraintSolver

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

@@ -155,8 +155,7 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
 		body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
 		body2.getLinearVelocity(),
 		body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
-	btScalar a;
-	a=jacDiagABInv;
+
 
 
 	rel_vel = normal.dot(vel);

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

@@ -23,9 +23,8 @@ subject to the following restrictions:
 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();
+	m_frameInA = frameInA;
+	m_frameInB = frameInB;
 
 }
 
@@ -37,14 +36,16 @@ btFixedConstraint::~btFixedConstraint ()
 void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
 {
 	info->m_numConstraintRows = 6;
-	info->nub = 6;
+	info->nub = 0;
 }
 
 void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
 {
 	//fix the 3 linear degrees of freedom
 
-	
+	const btTransform& transA = m_rbA.getCenterOfMassTransform();
+	const btTransform& transB = m_rbB.getCenterOfMassTransform();
+
 	const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
 	const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
 	const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
@@ -55,15 +56,15 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
 	info->m_J1linearAxis[info->rowskip+1] = 1;
 	info->m_J1linearAxis[2*info->rowskip+2] = 1;
 
-	btVector3 a1 = worldOrnA*m_pivotInA;
-	{
+	btVector3 a1 = worldOrnA * m_frameInA.getOrigin();
+    {
 		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;
@@ -71,10 +72,8 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
 		info->m_J2linearAxis[2*info->rowskip+2] = -1;
 	}
 	
-	btVector3 a2 = worldOrnB*m_pivotInB;
-   
-	{
-	//	btVector3 a2n = -a2;
+	btVector3 a2 = worldOrnB*m_frameInB.getOrigin();
+   {
 		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
 		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
 		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
@@ -88,42 +87,100 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
     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];
-    }
-
+	btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0);
+	btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1);
+	btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2);
+	btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0);
+	btVector3 target;
+	//btScalar x = ivB.dot(ivA);//??
+	btScalar y = ivB.dot(jvA);
+	btScalar z = ivB.dot(kvA);
+	btVector3 swingAxis(0,0,0);
+	{ 
+		if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
+		{
+			swingAxis = -ivB.cross(ivA);
+		}
+	}
+	btVector3 vTwist(1,0,0);
+
+	// compute rotation of A wrt B (in constraint space)
+	btQuaternion qA = transA.getRotation() * m_frameInA.getRotation();
+	btQuaternion qB = transB.getRotation() * m_frameInB.getRotation();
+	btQuaternion qAB = qB.inverse() * qA;
+	// split rotation into cone and twist
+	// (all this is done from B's perspective. Maybe I should be averaging axes...)
+	btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
+	btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
+	btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
+
+	int row = 3;
+    int srow = row * info->rowskip;
+	btVector3 ax1;
+	// angular limits
+	{
+		btScalar *J1 = info->m_J1angularAxis;
+		btScalar *J2 = info->m_J2angularAxis;
+		btTransform trA = transA*m_frameInA;
+		btVector3 twistAxis = trA.getBasis().getColumn(0);
+
+		btVector3 p = trA.getBasis().getColumn(1);
+		btVector3 q = trA.getBasis().getColumn(2);
+		int srow1 = srow + info->rowskip;
+		J1[srow+0] = p[0];
+		J1[srow+1] = p[1];
+		J1[srow+2] = p[2];
+		J1[srow1+0] = q[0];
+		J1[srow1+1] = q[1];
+		J1[srow1+2] = q[2];
+		J2[srow+0] = -p[0];
+		J2[srow+1] = -p[1];
+		J2[srow+2] = -p[2];
+		J2[srow1+0] = -q[0];
+		J2[srow1+1] = -q[1];
+		J2[srow1+2] = -q[2];
+		btScalar fact = info->fps;
+		info->m_constraintError[srow] =   fact * swingAxis.dot(p);
+		info->m_constraintError[srow1] =  fact * swingAxis.dot(q);
+		info->m_lowerLimit[srow] = -SIMD_INFINITY;
+		info->m_upperLimit[srow] = SIMD_INFINITY;
+		info->m_lowerLimit[srow1] = -SIMD_INFINITY;
+		info->m_upperLimit[srow1] = SIMD_INFINITY;
+		srow = srow1 + info->rowskip;
+
+		{
+			btQuaternion qMinTwist = qABTwist;
+			btScalar twistAngle = qABTwist.getAngle();
+
+			if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
+			{
+				qMinTwist = -(qABTwist);
+				twistAngle = qMinTwist.getAngle();
+			}
+
+			if (twistAngle > SIMD_EPSILON)
+			{
+				twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
+				twistAxis.normalize();
+				twistAxis = quatRotate(qB, -twistAxis);
+			}
+			ax1 = twistAxis;
+			btScalar *J1 = info->m_J1angularAxis;
+			btScalar *J2 = info->m_J2angularAxis;
+			J1[srow+0] = ax1[0];
+			J1[srow+1] = ax1[1];
+			J1[srow+2] = ax1[2];
+			J2[srow+0] = -ax1[0];
+			J2[srow+1] = -ax1[1];
+			J2[srow+2] = -ax1[2];
+			btScalar k = info->fps;
+			info->m_constraintError[srow] = k * twistAngle;
+			info->m_lowerLimit[srow] = -SIMD_INFINITY;
+			info->m_upperLimit[srow] = SIMD_INFINITY;
+		}
+	}
 }

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

@@ -20,10 +20,9 @@ subject to the following restrictions:
 
 ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
 {
-	btVector3 m_pivotInA;
-	btVector3 m_pivotInB;
-	btQuaternion m_relTargetAB;
 
+	btTransform m_frameInA;
+	btTransform m_frameInB;
 public:
 	btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
 	

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

@@ -1,54 +1,54 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2012 Advanced Micro Devices, Inc.  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou.
-
-#include "btGearConstraint.h"
-
-btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio)
-:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB),
-m_axisInA(axisInA),
-m_axisInB(axisInB),
-m_ratio(ratio)
-{
-}
-
-btGearConstraint::~btGearConstraint ()
-{
-}
-
-void btGearConstraint::getInfo1 (btConstraintInfo1* info)
-{
-	info->m_numConstraintRows = 1;
-	info->nub = 1;
-}
-
-void btGearConstraint::getInfo2 (btConstraintInfo2* info)
-{
-	btVector3 globalAxisA, globalAxisB;
-
-	globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA;
-	globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB;
-
-	info->m_J1angularAxis[0] = globalAxisA[0];
-	info->m_J1angularAxis[1] = globalAxisA[1];
-	info->m_J1angularAxis[2] = globalAxisA[2];
-
-	info->m_J2angularAxis[0] = m_ratio*globalAxisB[0];
-	info->m_J2angularAxis[1] = m_ratio*globalAxisB[1];
-	info->m_J2angularAxis[2] = m_ratio*globalAxisB[2];
-
-}
-
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2012 Advanced Micro Devices, Inc.  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou.
+
+#include "btGearConstraint.h"
+
+btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio)
+:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB),
+m_axisInA(axisInA),
+m_axisInB(axisInB),
+m_ratio(ratio)
+{
+}
+
+btGearConstraint::~btGearConstraint ()
+{
+}
+
+void btGearConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	info->m_numConstraintRows = 1;
+	info->nub = 1;
+}
+
+void btGearConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btVector3 globalAxisA, globalAxisB;
+
+	globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA;
+	globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB;
+
+	info->m_J1angularAxis[0] = globalAxisA[0];
+	info->m_J1angularAxis[1] = globalAxisA[1];
+	info->m_J1angularAxis[2] = globalAxisA[2];
+
+	info->m_J2angularAxis[0] = m_ratio*globalAxisB[0];
+	info->m_J2angularAxis[1] = m_ratio*globalAxisB[1];
+	info->m_J2angularAxis[2] = m_ratio*globalAxisB[2];
+
+}
+

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

@@ -1,79 +1,79 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2012 Advanced Micro Devices, Inc.  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-
-#ifndef BT_GEAR_CONSTRAINT_H
-#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
-{
-protected:
-	btVector3	m_axisInA;
-	btVector3	m_axisInB;
-	bool		m_useFrameA;
-	btScalar	m_ratio;
-
-public:
-	btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f);
-	virtual ~btGearConstraint ();
-
-	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo1 (btConstraintInfo1* info);
-
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2012 Advanced Micro Devices, Inc.  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef BT_GEAR_CONSTRAINT_H
+#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
+{
+protected:
+	btVector3	m_axisInA;
+	btVector3	m_axisInB;
+	bool		m_useFrameA;
+	btScalar	m_ratio;
+
+public:
+	btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f);
+	virtual ~btGearConstraint ();
+
+	///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);
 
-	void setAxisA(btVector3& axisA) 
-	{
-		m_axisInA = axisA;
+	void setAxisA(btVector3& axisA) 
+	{
+		m_axisInA = axisA;
 	}
-	void setAxisB(btVector3& axisB)
-	{
-		m_axisInB = axisB;
+	void setAxisB(btVector3& axisB)
+	{
+		m_axisInB = axisB;
 	}
-	void setRatio(btScalar ratio)
-	{
-		m_ratio = ratio;
+	void setRatio(btScalar ratio)
+	{
+		m_ratio = ratio;
 	}
-	const btVector3& getAxisA() const
-	{
-		return m_axisInA;
+	const btVector3& getAxisA() const
+	{
+		return m_axisInA;
 	}
-	const btVector3& getAxisB() const
-	{
-		return m_axisInB;
+	const btVector3& getAxisB() const
+	{
+		return m_axisInB;
 	}
-	btScalar getRatio() const
-	{
-		return m_ratio;
+	btScalar getRatio() const
+	{
+		return m_ratio;
 	}
 
 
@@ -84,69 +84,69 @@ public:
 		(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
+
+	///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/btGeneric6DofSpring2Constraint.cpp

@@ -0,0 +1,1113 @@
+/*
+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.
+*/
+
+/*
+2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer
+Pros:
+- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled)
+- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring)
+- Servo motor functionality
+- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision)
+- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2)
+
+Cons:
+- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. (with PGS)
+- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.)
+*/
+
+/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
+/// Added support for generic constraint solver through getInfo1/getInfo2 methods
+
+/*
+2007-09-09
+btGeneric6DofConstraint Refactored by Francisco Le?n
+email: [email protected]
+http://gimpact.sf.net
+*/
+
+
+
+#include "btGeneric6DofSpring2Constraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include <new>
+
+
+
+btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder)
+	: btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB)
+	, m_frameInA(frameInA)
+	, m_frameInB(frameInB)
+	, m_rotateOrder(rotOrder)	
+	, m_flags(0)
+{
+	calculateTransforms();
+}
+
+
+btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder)
+	: btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB)
+	, m_frameInB(frameInB)
+	, m_rotateOrder(rotOrder)
+	, m_flags(0)
+{
+	///not providing rigidbody A means implicitly using worldspace for body A
+	m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
+	calculateTransforms();
+}
+
+
+btScalar btGeneric6DofSpring2Constraint::btGetMatrixElem(const btMatrix3x3& mat, int index)
+{
+	int i = index%3;
+	int j = index/3;
+	return mat[i][j];
+}
+
+// MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
+
+bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
+{
+	// rot =  cy*cz          -cy*sz           sy
+	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+
+	btScalar fi = btGetMatrixElem(mat,2);
+	if (fi < btScalar(1.0f))
+	{
+		if (fi > btScalar(-1.0f))
+		{
+			xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
+			xyz[1] = btAsin(btGetMatrixElem(mat,2));
+			xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+			return true;
+		}
+		else
+		{
+			// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+			xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+			xyz[1] = -SIMD_HALF_PI;
+			xyz[2] = btScalar(0.0);
+			return false;
+		}
+	}
+	else
+	{
+		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+		xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+		xyz[1] = SIMD_HALF_PI;
+		xyz[2] = 0.0;
+	}
+	return false;
+}
+
+bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz)
+{
+	// rot =  cy*cz          -sz           sy*cz
+	//        cy*cx*sz+sx*sy  cx*cz        sy*cx*sz-cy*sx
+	//        cy*sx*sz-cx*sy  sx*cz        sy*sx*sz+cx*cy
+
+	btScalar fi = btGetMatrixElem(mat,1);
+	if (fi < btScalar(1.0f))
+	{
+		if (fi > btScalar(-1.0f))
+		{
+			xyz[0] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,4));
+			xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
+			xyz[2] = btAsin(-btGetMatrixElem(mat,1));
+			return true;
+		}
+		else
+		{
+			xyz[0] = -btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
+			xyz[1] = btScalar(0.0);
+			xyz[2] = SIMD_HALF_PI;
+			return false;
+		}
+	}
+	else
+	{
+		xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
+		xyz[1] = 0.0;
+		xyz[2] = -SIMD_HALF_PI;
+	}
+	return false;
+}
+
+bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz)
+{
+	// rot =  cy*cz+sy*sx*sz  cz*sy*sx-cy*sz  cx*sy
+	//        cx*sz           cx*cz           -sx
+	//        cy*sx*sz-cz*sy  sy*sz+cy*cz*sx  cy*cx
+
+	btScalar fi = btGetMatrixElem(mat,5);
+	if (fi < btScalar(1.0f))
+	{
+		if (fi > btScalar(-1.0f))
+		{
+			xyz[0] = btAsin(-btGetMatrixElem(mat,5));
+			xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,8));
+			xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+			return true;
+		}
+		else
+		{
+			xyz[0] = SIMD_HALF_PI;
+			xyz[1] = -btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+			xyz[2] = btScalar(0.0);
+			return false;
+		}
+	}
+	else
+	{
+		xyz[0] = -SIMD_HALF_PI;
+		xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+		xyz[2] = 0.0;
+	}
+	return false;
+}
+
+bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz)
+{
+	// rot =  cy*cz   sy*sx-cy*cx*sz   cx*sy+cy*sz*sx
+	//        sz           cz*cx           -cz*sx
+	//        -cz*sy  cy*sx+cx*sy*sz   cy*cx-sy*sz*sx
+
+	btScalar fi = btGetMatrixElem(mat,3);
+	if (fi < btScalar(1.0f))
+	{
+		if (fi > btScalar(-1.0f))
+		{
+			xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,4));
+			xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,0));
+			xyz[2] = btAsin(btGetMatrixElem(mat,3));
+			return true;
+		}
+		else
+		{
+			xyz[0] = btScalar(0.0);
+			xyz[1] = -btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
+			xyz[2] = -SIMD_HALF_PI;
+			return false;
+		}
+	}
+	else
+	{
+		xyz[0] = btScalar(0.0);
+		xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
+		xyz[2] = SIMD_HALF_PI;
+	}
+	return false;
+}
+
+bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz)
+{
+	// rot =  cz*cy-sz*sx*sy    -cx*sz   cz*sy+cy*sz*sx
+	//        cy*sz+cz*sx*sy     cz*cx   sz*sy-cz*xy*sx
+	//        -cx*sy              sx     cx*cy
+
+	btScalar fi = btGetMatrixElem(mat,7);
+	if (fi < btScalar(1.0f))
+	{
+		if (fi > btScalar(-1.0f))
+		{
+			xyz[0] = btAsin(btGetMatrixElem(mat,7));
+			xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
+			xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,4));
+			return true;
+		}
+		else
+		{
+			xyz[0] = -SIMD_HALF_PI;
+			xyz[1] = btScalar(0.0);
+			xyz[2] = -btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
+			return false;
+		}
+	}
+	else
+	{
+		xyz[0] = SIMD_HALF_PI;
+		xyz[1] = btScalar(0.0);
+		xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
+	}
+	return false;
+}
+
+bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz)
+{
+	// rot =  cz*cy   cz*sy*sx-cx*sz   sz*sx+cz*cx*sy
+	//        cy*sz   cz*cx+sz*sy*sx   cx*sz*sy-cz*sx
+	//        -sy          cy*sx         cy*cx
+
+	btScalar fi = btGetMatrixElem(mat,6);
+	if (fi < btScalar(1.0f))
+	{
+		if (fi > btScalar(-1.0f))
+		{
+			xyz[0] = btAtan2(btGetMatrixElem(mat,7), btGetMatrixElem(mat,8));
+			xyz[1] = btAsin(-btGetMatrixElem(mat,6));
+			xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,0));
+			return true;
+		}
+		else
+		{
+			xyz[0] = btScalar(0.0);
+			xyz[1] = SIMD_HALF_PI;
+			xyz[2] = -btAtan2(btGetMatrixElem(mat,1),btGetMatrixElem(mat,2));
+			return false;
+		}
+	}
+	else
+	{
+		xyz[0] = btScalar(0.0);
+		xyz[1] = -SIMD_HALF_PI;
+		xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2));
+	}
+	return false;
+}
+
+void btGeneric6DofSpring2Constraint::calculateAngleInfo()
+{
+	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
+	switch (m_rotateOrder)
+	{
+		case RO_XYZ : matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); break;
+		case RO_XZY : matrixToEulerXZY(relative_frame,m_calculatedAxisAngleDiff); break;
+		case RO_YXZ : matrixToEulerYXZ(relative_frame,m_calculatedAxisAngleDiff); break;
+		case RO_YZX : matrixToEulerYZX(relative_frame,m_calculatedAxisAngleDiff); break;
+		case RO_ZXY : matrixToEulerZXY(relative_frame,m_calculatedAxisAngleDiff); break;
+		case RO_ZYX : matrixToEulerZYX(relative_frame,m_calculatedAxisAngleDiff); break;
+		default : btAssert(false);
+	}
+	// in euler angle mode we do not actually constrain the angular velocity
+	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
+	//
+	//    to get			constrain w2-w1 along		...not
+	//    ------			---------------------		------
+	//    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
+	//    d(angle[1])/dt = 0	ax[1]
+	//    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
+	//
+	// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+	// to prove the result for angle[0], write the expression for angle[0] from
+	// GetInfo1 then take the derivative. to prove this for angle[2] it is
+	// easier to take the euler rate expression for d(angle[2])/dt with respect
+	// to the components of w and set that to 0.
+	switch (m_rotateOrder)
+	{
+	case RO_XYZ :
+		{
+			//Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles)
+			//The two planes are non-homologous, so this is a Tait–Bryan angle formalism and not a proper Euler
+			//Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation)
+			//that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait–Bryan angles)
+			// x' = Nperp = N.cross(axis2)
+			// y' = N = axis2.cross(axis0)	
+			// z' = z
+			//
+			// x" = X
+			// y" = y'
+			// z" = ??
+			//in other words:
+			//first rotate around z
+			//second rotate around y'= z.cross(X)
+			//third rotate around x" = X
+			//Original XYZ extrinsic rotation order. 
+			//Planes: xy and YZ normals: z, X.  Plane intersection (N) is z.cross(X)
+			btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
+			btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
+			m_calculatedAxis[1] = axis2.cross(axis0);
+			m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
+			m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
+			break;
+		}
+	case RO_XZY :
+		{
+			//planes: xz,ZY normals: y, X
+			//first rotate around y
+			//second rotate around z'= y.cross(X)
+			//third rotate around x" = X
+			btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
+			btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1);
+			m_calculatedAxis[2] = axis0.cross(axis1);
+			m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]);
+			m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
+			break;
+		}
+	case RO_YXZ :
+		{
+			//planes: yx,XZ normals: z, Y
+			//first rotate around z
+			//second rotate around x'= z.cross(Y)
+			//third rotate around y" = Y
+			btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1);
+			btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
+			m_calculatedAxis[0] = axis1.cross(axis2);
+			m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]);
+			m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
+			break;
+		}
+	case RO_YZX :
+		{
+			//planes: yz,ZX normals: x, Y
+			//first rotate around x
+			//second rotate around z'= x.cross(Y)
+			//third rotate around y" = Y
+			btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0);
+			btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1);
+			m_calculatedAxis[2] = axis0.cross(axis1);
+			m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]);
+			m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
+			break;
+		}
+	case RO_ZXY :
+		{
+			//planes: zx,XY normals: y, Z
+			//first rotate around y
+			//second rotate around x'= y.cross(Z)
+			//third rotate around z" = Z
+			btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1);
+			btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2);
+			m_calculatedAxis[0] = axis1.cross(axis2);
+			m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]);
+			m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
+			break;
+		}
+	case RO_ZYX :
+		{
+			//planes: zy,YX normals: x, Z
+			//first rotate around x
+			//second rotate around y' = x.cross(Z)
+			//third rotate around z" = Z
+			btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0);
+			btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2);
+			m_calculatedAxis[1] = axis2.cross(axis0);
+			m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
+			m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
+			break;
+		}
+	default:
+		btAssert(false);
+	}
+
+	m_calculatedAxis[0].normalize();
+	m_calculatedAxis[1].normalize();
+	m_calculatedAxis[2].normalize();
+
+}
+
+void btGeneric6DofSpring2Constraint::calculateTransforms()
+{
+	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+
+void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+{
+	m_calculatedTransformA = transA * m_frameInA;
+	m_calculatedTransformB = transB * m_frameInB;
+	calculateLinearInfo();
+	calculateAngleInfo();
+
+	btScalar miA = getRigidBodyA().getInvMass();
+	btScalar miB = getRigidBodyB().getInvMass();
+	m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+	btScalar miS = miA + miB;
+	if(miS > btScalar(0.f))
+	{
+		m_factA = miB / miS;
+	}
+	else 
+	{
+		m_factA = btScalar(0.5f);
+	}
+	m_factB = btScalar(1.0f) - m_factA;
+}
+
+
+void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index)
+{
+	btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+	angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
+	m_angularLimits[axis_index].m_currentPosition = angle;
+	m_angularLimits[axis_index].testLimitValue(angle);
+}
+
+
+void btGeneric6DofSpring2Constraint::getInfo1 (btConstraintInfo1* info)
+{
+	//prepare constraint
+	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	info->m_numConstraintRows = 0;
+	info->nub = 0;
+	int i;
+	//test linear limits
+	for(i = 0; i < 3; i++)
+	{
+		     if (m_linearLimits.m_currentLimit[i]==4) info->m_numConstraintRows += 2;
+		else if (m_linearLimits.m_currentLimit[i]!=0) info->m_numConstraintRows += 1;
+		if (m_linearLimits.m_enableMotor[i] ) info->m_numConstraintRows += 1;
+		if (m_linearLimits.m_enableSpring[i]) info->m_numConstraintRows += 1;
+	}
+	//test angular limits
+	for (i=0;i<3 ;i++ )
+	{
+		testAngularLimitMotor(i);
+		     if (m_angularLimits[i].m_currentLimit==4) info->m_numConstraintRows += 2;
+		else if (m_angularLimits[i].m_currentLimit!=0) info->m_numConstraintRows += 1;
+		if (m_angularLimits[i].m_enableMotor ) info->m_numConstraintRows += 1;
+		if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1;
+	}
+}
+
+
+void btGeneric6DofSpring2Constraint::getInfo2 (btConstraintInfo2* info)
+{
+	const btTransform& transA = m_rbA.getCenterOfMassTransform();
+	const btTransform& transB = m_rbB.getCenterOfMassTransform();
+	const btVector3& linVelA = m_rbA.getLinearVelocity();
+	const btVector3& linVelB = m_rbB.getLinearVelocity();
+	const btVector3& angVelA = m_rbA.getAngularVelocity();
+	const btVector3& angVelB = m_rbB.getAngularVelocity();
+
+	// for stability better to solve angular limits first
+	int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
+	setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+}
+
+
+int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+	//solve linear limits
+	btRotationalLimitMotor2 limot;
+	for (int i=0;i<3 ;i++ )
+	{
+		if(m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i])
+		{ // re-use rotational motor code
+			limot.m_bounce              = m_linearLimits.m_bounce[i];
+			limot.m_currentLimit        = m_linearLimits.m_currentLimit[i];
+			limot.m_currentPosition     = m_linearLimits.m_currentLinearDiff[i];
+			limot.m_currentLimitError   = m_linearLimits.m_currentLimitError[i];
+			limot.m_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i];
+			limot.m_enableMotor         = m_linearLimits.m_enableMotor[i];
+			limot.m_servoMotor          = m_linearLimits.m_servoMotor[i];
+			limot.m_servoTarget         = m_linearLimits.m_servoTarget[i];
+			limot.m_enableSpring        = m_linearLimits.m_enableSpring[i];
+			limot.m_springStiffness     = m_linearLimits.m_springStiffness[i];
+			limot.m_springDamping       = m_linearLimits.m_springDamping[i];
+			limot.m_equilibriumPoint    = m_linearLimits.m_equilibriumPoint[i];
+			limot.m_hiLimit             = m_linearLimits.m_upperLimit[i];
+			limot.m_loLimit             = m_linearLimits.m_lowerLimit[i];
+			limot.m_maxMotorForce       = m_linearLimits.m_maxMotorForce[i];
+			limot.m_targetVelocity      = m_linearLimits.m_targetVelocity[i];
+			btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
+			int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2);
+			limot.m_stopCFM  = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
+			limot.m_stopERP  = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp;
+			limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0];
+			limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp;
+
+			//rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible.
+			int indx1 = (i + 1) % 3;
+			int indx2 = (i + 2) % 3;
+			int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static)
+			#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3
+			bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 ||
+				m_angularLimits[indx1].m_currentLimit == 2 ||
+				( m_angularLimits[indx1].m_currentLimit == 3 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) ||
+				( m_angularLimits[indx1].m_currentLimit == 4 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) );
+			bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 ||
+				m_angularLimits[indx2].m_currentLimit == 2 ||
+				( m_angularLimits[indx2].m_currentLimit == 3 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) ||
+				( m_angularLimits[indx2].m_currentLimit == 4 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) );
+			if( indx1Violated && indx2Violated )
+			{
+				rotAllowed = 0;
+			}
+			row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
+
+		}
+	}
+	return row;
+}
+
+
+
+int btGeneric6DofSpring2Constraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+	int row = row_offset;
+
+	//order of rotational constraint rows
+	int cIdx[] = {0, 1, 2};
+	switch(m_rotateOrder)
+	{
+		case RO_XYZ : cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break;
+		case RO_XZY : cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break;
+		case RO_YXZ : cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break;
+		case RO_YZX : cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break;
+		case RO_ZXY : cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break;
+		case RO_ZYX : cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break;
+		default : btAssert(false);
+	}
+
+	for (int ii = 0; ii < 3 ; ii++ )
+	{
+		int i = cIdx[ii];
+		if(m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring)
+		{
+			btVector3 axis = getAxis(i);
+			int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2);
+			if(!(flags & BT_6DOF_FLAGS_CFM_STOP2))
+			{
+				m_angularLimits[i].m_stopCFM = info->cfm[0];
+			}
+			if(!(flags & BT_6DOF_FLAGS_ERP_STOP2))
+			{
+				m_angularLimits[i].m_stopERP = info->erp;
+			}
+			if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
+			{
+				m_angularLimits[i].m_motorCFM = info->cfm[0];
+			}
+			if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
+			{
+				m_angularLimits[i].m_motorERP = info->erp;
+			}
+			row += get_limit_motor_info2(&m_angularLimits[i],transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
+		}
+	}
+
+	return row;
+}
+
+
+void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const btTransform& frameB)
+{
+	m_frameInA = frameA;
+	m_frameInB = frameB;
+	buildJacobian();
+	calculateTransforms();
+}
+
+
+void btGeneric6DofSpring2Constraint::calculateLinearInfo()
+{
+	m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
+	m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
+	for(int i = 0; i < 3; i++)
+	{
+		m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
+		m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
+	}
+}
+
+void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2 *info, int srow, btVector3& ax1, int rotational, int rotAllowed)
+{
+	btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+	btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
+
+	J1[srow+0] = ax1[0];
+	J1[srow+1] = ax1[1];
+	J1[srow+2] = ax1[2];
+
+	J2[srow+0] = -ax1[0];
+	J2[srow+1] = -ax1[1];
+	J2[srow+2] = -ax1[2];
+
+	if(!rotational)
+	{
+		btVector3 tmpA, tmpB, relA, relB;
+		// get vector from bodyB to frameB in WCS
+		relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
+		// same for bodyA
+		relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
+		tmpA = relA.cross(ax1);
+		tmpB = relB.cross(ax1);
+		if(m_hasStaticBody && (!rotAllowed))
+		{
+			tmpA *= m_factA;
+			tmpB *= m_factB;
+		}
+		int i;
+		for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
+		for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
+	}
+}
+
+
+int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
+	btRotationalLimitMotor2 * limot,
+	const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+	btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
+{
+	int count = 0;
+	int srow = row * info->rowskip;
+
+	if (limot->m_currentLimit==4) 
+	{
+		btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
+
+		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
+		if (rotational) {
+			if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
+				btScalar bounceerror = -limot->m_bounce* vel;
+				if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
+			}
+		} else {
+			if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
+				btScalar bounceerror = -limot->m_bounce* vel;
+				if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
+			}
+		}
+		info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY;
+		info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0;
+		info->cfm[srow] = limot->m_stopCFM;
+		srow += info->rowskip;
+		++count;
+
+		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1);
+		if (rotational) {
+			if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
+				btScalar bounceerror = -limot->m_bounce* vel;
+				if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
+			}
+		} else {
+			if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
+				btScalar bounceerror = -limot->m_bounce* vel;
+				if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
+			}
+		}
+		info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0;
+		info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY;
+		info->cfm[srow] = limot->m_stopCFM;
+		srow += info->rowskip;
+		++count;
+	} else
+	if (limot->m_currentLimit==3) 
+	{
+		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
+		info->m_lowerLimit[srow] = -SIMD_INFINITY;
+		info->m_upperLimit[srow] = SIMD_INFINITY;
+		info->cfm[srow] = limot->m_stopCFM;
+		srow += info->rowskip;
+		++count;
+	}
+
+	if (limot->m_enableMotor && !limot->m_servoMotor)
+	{
+		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
+		btScalar mot_fact = getMotorFactor(limot->m_currentPosition, 
+			limot->m_loLimit,
+			limot->m_hiLimit,
+			tag_vel,
+			info->fps * limot->m_motorERP);
+		info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
+		info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
+		info->m_upperLimit[srow] = limot->m_maxMotorForce;
+		info->cfm[srow] = limot->m_motorCFM;
+		srow += info->rowskip;
+		++count;
+	}
+
+	if (limot->m_enableMotor && limot->m_servoMotor)
+	{
+		btScalar error = limot->m_currentPosition - limot->m_servoTarget;
+		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
+		btScalar tag_vel = -targetvelocity;
+		btScalar mot_fact;
+		if(error != 0)
+		{
+			btScalar lowLimit;
+			btScalar hiLimit;
+			if(limot->m_loLimit > limot->m_hiLimit)
+			{
+				lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY;
+				hiLimit  = error < 0 ? limot->m_servoTarget :  SIMD_INFINITY;
+			}
+			else
+			{
+				lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit;
+				hiLimit  = error < 0 && limot->m_servoTarget<limot->m_hiLimit ? limot->m_servoTarget : limot->m_hiLimit;
+			}
+			mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
+		} 
+		else 
+		{
+			mot_fact = 0;
+		}
+		info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
+		info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
+		info->m_upperLimit[srow] = limot->m_maxMotorForce;
+		info->cfm[srow] = limot->m_motorCFM;
+		srow += info->rowskip;
+		++count;
+	}
+
+	if (limot->m_enableSpring)
+	{
+		btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint;
+		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+
+		//btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping);
+		//if(cfm > 0.99999)
+		//	cfm = 0.99999;
+		//btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping);
+		//info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0);
+		//info->m_lowerLimit[srow] = -SIMD_INFINITY;
+		//info->m_upperLimit[srow] = SIMD_INFINITY;
+
+		btScalar dt = 1.0 / info->fps;
+		btScalar kd = limot->m_springDamping;
+		btScalar ks = limot->m_springStiffness;
+		btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
+//		btScalar erp = 0.1;
+		btScalar cfm = 0.0;
+		btScalar mA = 1.0 / m_rbA.getInvMass();
+		btScalar mB = 1.0 / m_rbB.getInvMass();
+		btScalar m = mA > mB ? mB : mA;
+		btScalar angularfreq = sqrt(ks / m);
+
+
+		//limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
+		if( 0.25 < angularfreq * dt)
+		{
+			ks = 1.0 / dt / dt / 16.0 / m;
+		}
+		//avoid overdamping
+		if(kd * dt > m)
+		{
+			kd = m / dt;
+		}
+		btScalar fs = ks * error * dt;
+		btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
+		btScalar f = (fs+fd);
+
+		info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ;
+
+		btScalar minf = f < fd ? f : fd;
+		btScalar maxf = f < fd ? fd : f;
+		if(!rotational)
+		{
+			info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
+			info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
+		}
+		else
+		{
+			info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf;
+			info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf;
+		}
+
+		info->cfm[srow] = cfm;
+		srow += info->rowskip;
+		++count;
+	}
+
+	return count;
+}
+
+
+//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+//If no axis is provided, it uses the default axis for this constraint.
+void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis)
+{
+	if((axis >= 0) && (axis < 3))
+	{
+		switch(num)
+		{
+			case BT_CONSTRAINT_STOP_ERP : 
+				m_linearLimits.m_stopERP[axis] = value;
+				m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			case BT_CONSTRAINT_STOP_CFM : 
+				m_linearLimits.m_stopCFM[axis] = value;
+				m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			case BT_CONSTRAINT_ERP : 
+				m_linearLimits.m_motorERP[axis] = value;
+				m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			case BT_CONSTRAINT_CFM : 
+				m_linearLimits.m_motorCFM[axis] = value;
+				m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			default : 
+				btAssertConstrParams(0);
+		}
+	}
+	else if((axis >=3) && (axis < 6))
+	{
+		switch(num)
+		{
+			case BT_CONSTRAINT_STOP_ERP : 
+				m_angularLimits[axis - 3].m_stopERP = value;
+				m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			case BT_CONSTRAINT_STOP_CFM : 
+				m_angularLimits[axis - 3].m_stopCFM = value;
+				m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			case BT_CONSTRAINT_ERP : 
+				m_angularLimits[axis - 3].m_motorERP = value;
+				m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			case BT_CONSTRAINT_CFM : 
+				m_angularLimits[axis - 3].m_motorCFM = value;
+				m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			default : 
+				btAssertConstrParams(0);
+		}
+	}
+	else
+	{
+		btAssertConstrParams(0);
+	}
+}
+
+//return the local value of parameter
+btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const 
+{
+	btScalar retVal = 0;
+	if((axis >= 0) && (axis < 3))
+	{
+		switch(num)
+		{
+			case BT_CONSTRAINT_STOP_ERP : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_linearLimits.m_stopERP[axis];
+				break;
+			case BT_CONSTRAINT_STOP_CFM : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_linearLimits.m_stopCFM[axis];
+				break;
+			case BT_CONSTRAINT_ERP : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_linearLimits.m_motorERP[axis];
+				break;
+			case BT_CONSTRAINT_CFM : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_linearLimits.m_motorCFM[axis];
+				break;
+			default : 
+				btAssertConstrParams(0);
+		}
+	}
+	else if((axis >=3) && (axis < 6))
+	{
+		switch(num)
+		{
+			case BT_CONSTRAINT_STOP_ERP : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_angularLimits[axis - 3].m_stopERP;
+				break;
+			case BT_CONSTRAINT_STOP_CFM : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_angularLimits[axis - 3].m_stopCFM;
+				break;
+			case BT_CONSTRAINT_ERP : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_angularLimits[axis - 3].m_motorERP;
+				break;
+			case BT_CONSTRAINT_CFM : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_angularLimits[axis - 3].m_motorCFM;
+				break;
+			default : 
+				btAssertConstrParams(0);
+		}
+	}
+	else
+	{
+		btAssertConstrParams(0);
+	}
+	return retVal;
+}
+
+ 
+
+void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1,const btVector3& axis2)
+{
+	btVector3 zAxis = axis1.normalized();
+	btVector3 yAxis = axis2.normalized();
+	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+	
+	btTransform frameInW;
+	frameInW.setIdentity();
+	frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
+	                              xAxis[1], yAxis[1], zAxis[1],
+	                              xAxis[2], yAxis[2], zAxis[2]);
+	
+	// now get constraint frame in local coordinate systems
+	m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
+	m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
+	
+	calculateTransforms();
+}
+
+void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_bounce[index] = bounce;
+	else
+		m_angularLimits[index - 3].m_bounce = bounce;
+}
+
+void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_enableMotor[index] = onOff;
+	else
+		m_angularLimits[index - 3].m_enableMotor = onOff;
+}
+
+void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_servoMotor[index] = onOff;
+	else
+		m_angularLimits[index - 3].m_servoMotor = onOff;
+}
+
+void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_targetVelocity[index] = velocity;
+	else
+		m_angularLimits[index - 3].m_targetVelocity = velocity;
+}
+
+void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_servoTarget[index] = target;
+	else
+		m_angularLimits[index - 3].m_servoTarget = target;
+}
+
+void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_maxMotorForce[index] = force;
+	else
+		m_angularLimits[index - 3].m_maxMotorForce = force;
+}
+
+void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_enableSpring[index] = onOff;
+	else
+		m_angularLimits[index - 3] .m_enableSpring = onOff;
+}
+
+void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_springStiffness[index] = stiffness;
+	else
+		m_angularLimits[index - 3] .m_springStiffness = stiffness;
+}
+
+void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_springDamping[index] = damping;
+	else
+		m_angularLimits[index - 3] .m_springDamping = damping;
+}
+
+void btGeneric6DofSpring2Constraint::setEquilibriumPoint()
+{
+	calculateTransforms();
+	int i;
+	for( i = 0; i < 3; i++)
+		m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
+	for(i = 0; i < 3; i++)
+		m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i];
+}
+
+void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index)
+{
+	btAssert((index >= 0) && (index < 6));
+	calculateTransforms();
+	if (index<3)
+		m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
+	else
+		m_angularLimits[index - 3] .m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3];
+}
+
+void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index, btScalar val)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_equilibriumPoint[index] = val;
+	else
+		m_angularLimits[index - 3] .m_equilibriumPoint = val;
+}
+
+
+//////////////////////////// btRotationalLimitMotor2 ////////////////////////////////////
+
+void btRotationalLimitMotor2::testLimitValue(btScalar test_value)
+{
+	//we can't normalize the angles here because we would lost the sign that we use later, but it doesn't seem to be a problem
+	if(m_loLimit > m_hiLimit) {
+		m_currentLimit = 0;
+		m_currentLimitError = btScalar(0.f);
+	}
+	else if(m_loLimit == m_hiLimit) {
+		m_currentLimitError = test_value - m_loLimit;
+		m_currentLimit = 3;
+	} else {
+		m_currentLimitError = test_value - m_loLimit;
+		m_currentLimitErrorHi = test_value - m_hiLimit;
+		m_currentLimit = 4;
+	}
+}
+
+//////////////////////////// btTranslationalLimitMotor2 ////////////////////////////////////
+
+void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_value)
+{
+	btScalar loLimit = m_lowerLimit[limitIndex];
+	btScalar hiLimit = m_upperLimit[limitIndex];
+	if(loLimit > hiLimit) {
+		m_currentLimitError[limitIndex] = 0;
+		m_currentLimit[limitIndex] = 0;
+	}
+	else if(loLimit == hiLimit) {
+		m_currentLimitError[limitIndex] = test_value - loLimit;
+		m_currentLimit[limitIndex] = 3;
+	} else {
+		m_currentLimitError[limitIndex] = test_value - loLimit;
+		m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
+		m_currentLimit[limitIndex] = 4;
+	}
+}
+
+

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

@@ -0,0 +1,654 @@
+/*
+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.
+*/
+
+/*
+2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer
+Pros:
+- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled)
+- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring)
+- Servo motor functionality
+- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision)
+- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2)
+
+Cons:
+- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation.
+- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.)
+*/
+
+/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
+/// Added support for generic constraint solver through getInfo1/getInfo2 methods
+
+/*
+2007-09-09
+btGeneric6DofConstraint Refactored by Francisco Le?n
+email: [email protected]
+http://gimpact.sf.net
+*/
+
+
+#ifndef BT_GENERIC_6DOF_CONSTRAINT2_H
+#define BT_GENERIC_6DOF_CONSTRAINT2_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btGeneric6DofSpring2ConstraintData2		btGeneric6DofSpring2ConstraintDoubleData2
+#define btGeneric6DofSpring2ConstraintDataName	"btGeneric6DofSpring2ConstraintDoubleData2"
+#else
+#define btGeneric6DofSpring2ConstraintData2		btGeneric6DofSpring2ConstraintData
+#define btGeneric6DofSpring2ConstraintDataName	"btGeneric6DofSpring2ConstraintData"
+#endif //BT_USE_DOUBLE_PRECISION
+
+enum RotateOrder
+{
+	RO_XYZ,
+	RO_XZY,
+	RO_YXZ,
+	RO_YZX,
+	RO_ZXY,
+	RO_ZYX
+};
+
+class btRotationalLimitMotor2
+{
+public:
+// upper < lower means free
+// upper == lower means locked
+// upper > lower means limited
+	btScalar m_loLimit;
+	btScalar m_hiLimit;
+	btScalar m_bounce;
+	btScalar m_stopERP;
+	btScalar m_stopCFM;
+	btScalar m_motorERP;
+	btScalar m_motorCFM;
+	bool     m_enableMotor;
+	btScalar m_targetVelocity;
+	btScalar m_maxMotorForce;
+	bool     m_servoMotor;
+	btScalar m_servoTarget;
+	bool     m_enableSpring;
+	btScalar m_springStiffness;
+	btScalar m_springDamping;
+	btScalar m_equilibriumPoint;
+
+	btScalar m_currentLimitError;
+	btScalar m_currentLimitErrorHi;
+	btScalar m_currentPosition;
+	int      m_currentLimit;
+
+	btRotationalLimitMotor2()
+	{
+		m_loLimit             = 1.0f;
+		m_hiLimit             = -1.0f;
+		m_bounce              = 0.0f;
+		m_stopERP             = 0.2f;
+		m_stopCFM             = 0.f;
+		m_motorERP            = 0.9f;
+		m_motorCFM            = 0.f;
+		m_enableMotor         = false;
+		m_targetVelocity      = 0;
+		m_maxMotorForce       = 0.1f;
+		m_servoMotor          = false;
+		m_servoTarget         = 0;
+		m_enableSpring        = false;
+		m_springStiffness     = 0;
+		m_springDamping       = 0;
+		m_equilibriumPoint    = 0;
+
+		m_currentLimitError   = 0;
+		m_currentLimitErrorHi = 0;
+		m_currentPosition     = 0;
+		m_currentLimit        = 0;
+	}
+
+	btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot)
+	{
+		m_loLimit             = limot.m_loLimit;
+		m_hiLimit             = limot.m_hiLimit;
+		m_bounce              = limot.m_bounce;
+		m_stopERP             = limot.m_stopERP;
+		m_stopCFM             = limot.m_stopCFM;
+		m_motorERP            = limot.m_motorERP;
+		m_motorCFM            = limot.m_motorCFM;
+		m_enableMotor         = limot.m_enableMotor;
+		m_targetVelocity      = limot.m_targetVelocity;
+		m_maxMotorForce       = limot.m_maxMotorForce;
+		m_servoMotor          = limot.m_servoMotor;
+		m_servoTarget         = limot.m_servoTarget;
+		m_enableSpring        = limot.m_enableSpring;
+		m_springStiffness     = limot.m_springStiffness;
+		m_springDamping       = limot.m_springDamping;
+		m_equilibriumPoint    = limot.m_equilibriumPoint;
+
+		m_currentLimitError   = limot.m_currentLimitError;
+		m_currentLimitErrorHi = limot.m_currentLimitErrorHi;
+		m_currentPosition     = limot.m_currentPosition;
+		m_currentLimit        = limot.m_currentLimit;
+	}
+
+
+	bool isLimited()
+	{
+		if(m_loLimit > m_hiLimit) return false;
+		return true;
+	}
+
+	void testLimitValue(btScalar test_value);
+};
+
+
+
+class btTranslationalLimitMotor2
+{
+public:
+// upper < lower means free
+// upper == lower means locked
+// upper > lower means limited
+	btVector3 m_lowerLimit;
+	btVector3 m_upperLimit;
+	btVector3 m_bounce;
+	btVector3 m_stopERP;
+	btVector3 m_stopCFM;
+	btVector3 m_motorERP;
+	btVector3 m_motorCFM;
+	bool      m_enableMotor[3];
+	bool      m_servoMotor[3];
+	bool      m_enableSpring[3];
+	btVector3 m_servoTarget;
+	btVector3 m_springStiffness;
+	btVector3 m_springDamping;
+	btVector3 m_equilibriumPoint;
+	btVector3 m_targetVelocity;
+	btVector3 m_maxMotorForce;
+
+	btVector3 m_currentLimitError;
+	btVector3 m_currentLimitErrorHi;
+	btVector3 m_currentLinearDiff;
+	int       m_currentLimit[3];
+
+	btTranslationalLimitMotor2()
+	{
+		m_lowerLimit         .setValue(0.f , 0.f , 0.f );
+		m_upperLimit         .setValue(0.f , 0.f , 0.f );
+		m_bounce             .setValue(0.f , 0.f , 0.f );
+		m_stopERP            .setValue(0.2f, 0.2f, 0.2f);
+		m_stopCFM            .setValue(0.f , 0.f , 0.f );
+		m_motorERP           .setValue(0.9f, 0.9f, 0.9f);
+		m_motorCFM           .setValue(0.f , 0.f , 0.f );
+
+		m_currentLimitError  .setValue(0.f , 0.f , 0.f );
+		m_currentLimitErrorHi.setValue(0.f , 0.f , 0.f );
+		m_currentLinearDiff  .setValue(0.f , 0.f , 0.f );
+
+		for(int i=0; i < 3; i++) 
+		{
+			m_enableMotor[i]      = false;
+			m_servoMotor[i]       = false;
+			m_enableSpring[i]     = false;
+			m_servoTarget[i]      = btScalar(0.f);
+			m_springStiffness[i]  = btScalar(0.f);
+			m_springDamping[i]    = btScalar(0.f);
+			m_equilibriumPoint[i] = btScalar(0.f);
+			m_targetVelocity[i]   = btScalar(0.f);
+			m_maxMotorForce[i]    = btScalar(0.f);
+			
+			m_currentLimit[i]     = 0;
+		}
+	}
+
+	btTranslationalLimitMotor2(const btTranslationalLimitMotor2 & other )
+	{
+		m_lowerLimit          = other.m_lowerLimit;
+		m_upperLimit          = other.m_upperLimit;
+		m_bounce              = other.m_bounce;
+		m_stopERP             = other.m_stopERP;
+		m_stopCFM             = other.m_stopCFM;
+		m_motorERP            = other.m_motorERP;
+		m_motorCFM            = other.m_motorCFM;
+		
+		m_currentLimitError   = other.m_currentLimitError;
+		m_currentLimitErrorHi = other.m_currentLimitErrorHi;
+		m_currentLinearDiff   = other.m_currentLinearDiff;
+
+		for(int i=0; i < 3; i++) 
+		{
+			m_enableMotor[i]      = other.m_enableMotor[i];
+			m_servoMotor[i]       = other.m_servoMotor[i];
+			m_enableSpring[i]     = other.m_enableSpring[i];
+			m_servoTarget[i]      = other.m_servoTarget[i];
+			m_springStiffness[i]  = other.m_springStiffness[i];
+			m_springDamping[i]    = other.m_springDamping[i];
+			m_equilibriumPoint[i] = other.m_equilibriumPoint[i];
+			m_targetVelocity[i]   = other.m_targetVelocity[i];
+			m_maxMotorForce[i]    = other.m_maxMotorForce[i];
+
+			m_currentLimit[i]     = other.m_currentLimit[i];
+		}
+	}
+
+	inline bool isLimited(int limitIndex)
+	{
+		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+	}
+
+	void testLimitValue(int limitIndex, btScalar test_value);
+};
+
+enum bt6DofFlags2
+{
+	BT_6DOF_FLAGS_CFM_STOP2 = 1,
+	BT_6DOF_FLAGS_ERP_STOP2 = 2,
+	BT_6DOF_FLAGS_CFM_MOTO2 = 4,
+	BT_6DOF_FLAGS_ERP_MOTO2 = 8
+};
+#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis
+
+
+ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint
+{
+protected:
+
+	btTransform m_frameInA;
+	btTransform m_frameInB;
+
+	btJacobianEntry m_jacLinear[3];
+	btJacobianEntry m_jacAng[3];
+
+	btTranslationalLimitMotor2 m_linearLimits;
+	btRotationalLimitMotor2 m_angularLimits[3];
+
+	RotateOrder m_rotateOrder;
+
+protected:
+
+	btTransform  m_calculatedTransformA;
+	btTransform  m_calculatedTransformB;
+	btVector3    m_calculatedAxisAngleDiff;
+	btVector3    m_calculatedAxis[3];
+	btVector3    m_calculatedLinearDiff;
+	btScalar     m_factA;
+	btScalar     m_factB;
+	bool         m_hasStaticBody;
+	int          m_flags;
+
+	btGeneric6DofSpring2Constraint&	operator=(btGeneric6DofSpring2Constraint&)
+	{
+		btAssert(0);
+		return *this;
+	}
+
+	int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+	int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+
+	void calculateLinearInfo();
+	void calculateAngleInfo();
+	void testAngularLimitMotor(int axis_index);
+
+	void calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed);
+	int get_limit_motor_info2(btRotationalLimitMotor2* limot,
+		const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+		btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
+
+	static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
+	static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+    btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
+    btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
+
+	virtual void buildJacobian() {}
+	virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual int calculateSerializeBufferSize() const;
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
+
+	btRotationalLimitMotor2* getRotationalLimitMotor(int index) { return &m_angularLimits[index]; }
+	btTranslationalLimitMotor2* getTranslationalLimitMotor() { return &m_linearLimits; }
+
+	// Calculates the global transform for the joint offset for body A an B, and also calculates the angle differences between the bodies.
+	void calculateTransforms(const btTransform& transA,const btTransform& transB);
+	void calculateTransforms();
+
+	// Gets the global transform of the offset for body A
+	const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
+	// Gets the global transform of the offset for body B
+	const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
+
+	const btTransform & getFrameOffsetA() const { return m_frameInA; }
+	const btTransform & getFrameOffsetB() const { return m_frameInB; }
+
+	btTransform & getFrameOffsetA() { return m_frameInA; }
+	btTransform & getFrameOffsetB() { return m_frameInB; }
+
+	// Get the rotation axis in global coordinates ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
+	btVector3 getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; }
+
+	// Get the relative Euler angle ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
+	btScalar getAngle(int axis_index) const { return m_calculatedAxisAngleDiff[axis_index]; }
+
+	// Get the relative position of the constraint pivot ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
+	btScalar getRelativePivotPosition(int axis_index) const { return m_calculatedLinearDiff[axis_index]; }
+
+	void setFrames(const btTransform & frameA, const btTransform & frameB);
+
+	void setLinearLowerLimit(const btVector3& linearLower) { m_linearLimits.m_lowerLimit = linearLower; }
+	void getLinearLowerLimit(btVector3& linearLower) { linearLower = m_linearLimits.m_lowerLimit; }
+	void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; }
+	void getLinearUpperLimit(btVector3& linearUpper) { linearUpper = m_linearLimits.m_upperLimit; }
+
+	void setAngularLowerLimit(const btVector3& angularLower)
+	{
+		for(int i = 0; i < 3; i++) 
+			m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
+	}
+
+	void setAngularLowerLimitReversed(const btVector3& angularLower)
+	{
+		for(int i = 0; i < 3; i++) 
+			m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]);
+	}
+
+	void getAngularLowerLimit(btVector3& angularLower)
+	{
+		for(int i = 0; i < 3; i++) 
+			angularLower[i] = m_angularLimits[i].m_loLimit;
+	}
+
+	void getAngularLowerLimitReversed(btVector3& angularLower)
+	{
+		for(int i = 0; i < 3; i++)
+			angularLower[i] = -m_angularLimits[i].m_hiLimit;
+	}
+
+	void setAngularUpperLimit(const btVector3& angularUpper)
+	{
+		for(int i = 0; i < 3; i++)
+			m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
+	}
+
+	void setAngularUpperLimitReversed(const btVector3& angularUpper)
+	{
+		for(int i = 0; i < 3; i++)
+			m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]);
+	}
+
+	void getAngularUpperLimit(btVector3& angularUpper)
+	{
+		for(int i = 0; i < 3; i++)
+			angularUpper[i] = m_angularLimits[i].m_hiLimit;
+	}
+
+	void getAngularUpperLimitReversed(btVector3& angularUpper)
+	{
+		for(int i = 0; i < 3; i++)
+			angularUpper[i] = -m_angularLimits[i].m_loLimit;
+	}
+
+	//first 3 are linear, next 3 are angular
+
+	void setLimit(int axis, btScalar lo, btScalar hi)
+	{
+		if(axis<3)
+		{
+			m_linearLimits.m_lowerLimit[axis] = lo;
+			m_linearLimits.m_upperLimit[axis] = hi;
+		}
+		else
+		{
+			lo = btNormalizeAngle(lo);
+			hi = btNormalizeAngle(hi);
+			m_angularLimits[axis-3].m_loLimit = lo;
+			m_angularLimits[axis-3].m_hiLimit = hi;
+		}
+	}
+
+	void setLimitReversed(int axis, btScalar lo, btScalar hi)
+	{
+		if(axis<3)
+		{
+			m_linearLimits.m_lowerLimit[axis] = lo;
+			m_linearLimits.m_upperLimit[axis] = hi;
+		}
+		else
+		{
+			lo = btNormalizeAngle(lo);
+			hi = btNormalizeAngle(hi);
+			m_angularLimits[axis-3].m_hiLimit = -lo;
+			m_angularLimits[axis-3].m_loLimit = -hi;
+		}
+	}
+
+	bool isLimited(int limitIndex)
+	{
+		if(limitIndex<3)
+		{
+			return m_linearLimits.isLimited(limitIndex);
+		}
+		return m_angularLimits[limitIndex-3].isLimited();
+	}
+
+	void setRotationOrder(RotateOrder order) { m_rotateOrder = order; }
+	RotateOrder getRotationOrder() { return m_rotateOrder; }
+
+	void setAxis( const btVector3& axis1, const btVector3& axis2);
+
+	void setBounce(int index, btScalar bounce);
+
+	void enableMotor(int index, bool onOff);
+	void setServo(int index, bool onOff); // set the type of the motor (servo or not) (the motor has to be turned on for servo also)
+	void setTargetVelocity(int index, btScalar velocity);
+	void setServoTarget(int index, btScalar target);
+	void setMaxMotorForce(int index, btScalar force);
+
+	void enableSpring(int index, bool onOff);
+	void setStiffness(int index, btScalar stiffness);
+	void setDamping(int index, btScalar damping);
+	void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
+	void setEquilibriumPoint(int index);  // set the current constraint position/orientation as an equilibrium point for given DOF
+	void setEquilibriumPoint(int index, btScalar val);
+
+	//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	//If no axis is provided, it uses the default axis for this constraint.
+	virtual void setParam(int num, btScalar value, int axis = -1);
+	virtual btScalar getParam(int num, int axis = -1) const;
+};
+
+
+struct btGeneric6DofSpring2ConstraintData
+{
+	btTypedConstraintData m_typeConstraintData;
+	btTransformFloatData m_rbAFrame;
+	btTransformFloatData m_rbBFrame;
+
+	btVector3FloatData m_linearUpperLimit;
+	btVector3FloatData m_linearLowerLimit;
+	btVector3FloatData m_linearBounce;
+	btVector3FloatData m_linearStopERP;
+	btVector3FloatData m_linearStopCFM;
+	btVector3FloatData m_linearMotorERP;
+	btVector3FloatData m_linearMotorCFM;
+	btVector3FloatData m_linearTargetVelocity;
+	btVector3FloatData m_linearMaxMotorForce;
+	btVector3FloatData m_linearServoTarget;
+	btVector3FloatData m_linearSpringStiffness;
+	btVector3FloatData m_linearSpringDamping;
+	btVector3FloatData m_linearEquilibriumPoint;
+	char               m_linearEnableMotor[4];
+	char               m_linearServoMotor[4];
+	char               m_linearEnableSpring[4];
+	char               m_padding1[4];
+
+	btVector3FloatData m_angularUpperLimit;
+	btVector3FloatData m_angularLowerLimit;
+	btVector3FloatData m_angularBounce;
+	btVector3FloatData m_angularStopERP;
+	btVector3FloatData m_angularStopCFM;
+	btVector3FloatData m_angularMotorERP;
+	btVector3FloatData m_angularMotorCFM;
+	btVector3FloatData m_angularTargetVelocity;
+	btVector3FloatData m_angularMaxMotorForce;
+	btVector3FloatData m_angularServoTarget;
+	btVector3FloatData m_angularSpringStiffness;
+	btVector3FloatData m_angularSpringDamping;
+	btVector3FloatData m_angularEquilibriumPoint;
+	char               m_angularEnableMotor[4];
+	char               m_angularServoMotor[4];
+	char               m_angularEnableSpring[4];
+	char               m_padding2[4];
+
+	int                m_rotateOrder;
+	char               m_padding3[4];
+};
+
+struct btGeneric6DofSpring2ConstraintDoubleData2
+{
+	btTypedConstraintDoubleData m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame;
+	btTransformDoubleData m_rbBFrame;
+
+	btVector3DoubleData m_linearUpperLimit;
+	btVector3DoubleData m_linearLowerLimit;
+	btVector3DoubleData m_linearBounce;
+	btVector3DoubleData m_linearStopERP;
+	btVector3DoubleData m_linearStopCFM;
+	btVector3DoubleData m_linearMotorERP;
+	btVector3DoubleData m_linearMotorCFM;
+	btVector3DoubleData m_linearTargetVelocity;
+	btVector3DoubleData m_linearMaxMotorForce;
+	btVector3DoubleData m_linearServoTarget;
+	btVector3DoubleData m_linearSpringStiffness;
+	btVector3DoubleData m_linearSpringDamping;
+	btVector3DoubleData m_linearEquilibriumPoint;
+	char                m_linearEnableMotor[4];
+	char                m_linearServoMotor[4];
+	char                m_linearEnableSpring[4];
+	char                m_padding1[4];
+
+	btVector3DoubleData m_angularUpperLimit;
+	btVector3DoubleData m_angularLowerLimit;
+	btVector3DoubleData m_angularBounce;
+	btVector3DoubleData m_angularStopERP;
+	btVector3DoubleData m_angularStopCFM;
+	btVector3DoubleData m_angularMotorERP;
+	btVector3DoubleData m_angularMotorCFM;
+	btVector3DoubleData m_angularTargetVelocity;
+	btVector3DoubleData m_angularMaxMotorForce;
+	btVector3DoubleData m_angularServoTarget;
+	btVector3DoubleData m_angularSpringStiffness;
+	btVector3DoubleData m_angularSpringDamping;
+	btVector3DoubleData m_angularEquilibriumPoint;
+	char                m_angularEnableMotor[4];
+	char                m_angularServoMotor[4];
+	char                m_angularEnableSpring[4];
+	char                m_padding2[4];
+
+	int                 m_rotateOrder;
+	char                m_padding3[4];
+};
+
+SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btGeneric6DofSpring2ConstraintData2);
+}
+
+SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer;
+	btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
+
+	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]       = m_angularLimits[i].m_loLimit;
+		dof->m_angularUpperLimit.m_floats[i]       = m_angularLimits[i].m_hiLimit;
+		dof->m_angularBounce.m_floats[i]           = m_angularLimits[i].m_bounce;
+		dof->m_angularStopERP.m_floats[i]          = m_angularLimits[i].m_stopERP;
+		dof->m_angularStopCFM.m_floats[i]          = m_angularLimits[i].m_stopCFM;
+		dof->m_angularMotorERP.m_floats[i]         = m_angularLimits[i].m_motorERP;
+		dof->m_angularMotorCFM.m_floats[i]         = m_angularLimits[i].m_motorCFM;
+		dof->m_angularTargetVelocity.m_floats[i]   = m_angularLimits[i].m_targetVelocity;
+		dof->m_angularMaxMotorForce.m_floats[i]    = m_angularLimits[i].m_maxMotorForce;
+		dof->m_angularServoTarget.m_floats[i]      = m_angularLimits[i].m_servoTarget;
+		dof->m_angularSpringStiffness.m_floats[i]  = m_angularLimits[i].m_springStiffness;
+		dof->m_angularSpringDamping.m_floats[i]    = m_angularLimits[i].m_springDamping;
+		dof->m_angularEquilibriumPoint.m_floats[i] = m_angularLimits[i].m_equilibriumPoint;
+	}
+	dof->m_angularLowerLimit.m_floats[3]       = 0;
+	dof->m_angularUpperLimit.m_floats[3]       = 0;
+	dof->m_angularBounce.m_floats[3]           = 0;
+	dof->m_angularStopERP.m_floats[3]          = 0;
+	dof->m_angularStopCFM.m_floats[3]          = 0;
+	dof->m_angularMotorERP.m_floats[3]         = 0;
+	dof->m_angularMotorCFM.m_floats[3]         = 0;
+	dof->m_angularTargetVelocity.m_floats[3]   = 0;
+	dof->m_angularMaxMotorForce.m_floats[3]    = 0;
+	dof->m_angularServoTarget.m_floats[3]      = 0;
+	dof->m_angularSpringStiffness.m_floats[3]  = 0;
+	dof->m_angularSpringDamping.m_floats[3]    = 0;
+	dof->m_angularEquilibriumPoint.m_floats[3] = 0;
+	for (i=0;i<4;i++)
+	{
+		dof->m_angularEnableMotor[i]  = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0;
+		dof->m_angularServoMotor[i]   = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0;
+		dof->m_angularEnableSpring[i] = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0;
+	}
+
+	m_linearLimits.m_lowerLimit.serialize( dof->m_linearLowerLimit );
+	m_linearLimits.m_upperLimit.serialize( dof->m_linearUpperLimit );
+	m_linearLimits.m_bounce.serialize( dof->m_linearBounce );
+	m_linearLimits.m_stopERP.serialize( dof->m_linearStopERP );
+	m_linearLimits.m_stopCFM.serialize( dof->m_linearStopCFM );
+	m_linearLimits.m_motorERP.serialize( dof->m_linearMotorERP );
+	m_linearLimits.m_motorCFM.serialize( dof->m_linearMotorCFM );
+	m_linearLimits.m_targetVelocity.serialize( dof->m_linearTargetVelocity );
+	m_linearLimits.m_maxMotorForce.serialize( dof->m_linearMaxMotorForce );
+	m_linearLimits.m_servoTarget.serialize( dof->m_linearServoTarget );
+	m_linearLimits.m_springStiffness.serialize( dof->m_linearSpringStiffness );
+	m_linearLimits.m_springDamping.serialize( dof->m_linearSpringDamping );
+	m_linearLimits.m_equilibriumPoint.serialize( dof->m_linearEquilibriumPoint );
+	for (i=0;i<4;i++)
+	{
+		dof->m_linearEnableMotor[i]  = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0;
+		dof->m_linearServoMotor[i]   = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0;
+		dof->m_linearEnableSpring[i] = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0;
+	}
+
+	dof->m_rotateOrder = m_rotateOrder;
+
+	return btGeneric6DofSpring2ConstraintDataName;
+}
+
+
+
+
+
+#endif //BT_GENERIC_6DOF_CONSTRAINT_H

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

@@ -45,7 +45,11 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 									 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 									 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 									 m_useReferenceFrameA(useReferenceFrameA),
-									 m_flags(0)
+									 m_flags(0),
+									 m_normalCFM(0),
+									 m_normalERP(0),
+									 m_stopCFM(0),
+									 m_stopERP(0)
 {
 	m_rbAFrame.getOrigin() = pivotInA;
 	
@@ -101,7 +105,11 @@ m_angularOnly(false), m_enableAngularMotor(false),
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 m_useReferenceFrameA(useReferenceFrameA),
-m_flags(0)
+m_flags(0),
+m_normalCFM(0),
+m_normalERP(0),
+m_stopCFM(0),
+m_stopERP(0)
 {
 
 	// since no frame is given, assume this to be zero angle and just pick rb transform axis
@@ -151,7 +159,11 @@ m_enableAngularMotor(false),
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 m_useReferenceFrameA(useReferenceFrameA),
-m_flags(0)
+m_flags(0),
+m_normalCFM(0),
+m_normalERP(0),
+m_stopCFM(0),
+m_stopERP(0)
 {
 #ifndef	_BT_USE_CENTER_LIMIT_
 	//start with free
@@ -177,7 +189,11 @@ m_enableAngularMotor(false),
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 m_useReferenceFrameA(useReferenceFrameA),
-m_flags(0)
+m_flags(0),
+m_normalCFM(0),
+m_normalERP(0),
+m_stopCFM(0),
+m_stopERP(0)
 {
 	///not providing rigidbody B means implicitly using worldspace for body B
 
@@ -285,8 +301,60 @@ void	btHingeConstraint::buildJacobian()
 #endif //__SPU__
 
 
+static inline btScalar btNormalizeAnglePositive(btScalar angle)
+{
+  return btFmod(btFmod(angle, 2.0*SIMD_PI) + 2.0*SIMD_PI, 2.0*SIMD_PI);
+}
+
+
+
+static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
+{
+	btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
+	btNormalizeAnglePositive(accAngle)));
+	return result;
+}
+
+static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
+{
+	btScalar tol(0.3);
+	btScalar result = btShortestAngularDistance(accAngle, curAngle);
+
+	  if (btFabs(result) > tol)
+		return curAngle;
+	  else
+		return accAngle + result;
+
+	return curAngle;
+}
+
+
+btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle()
+{
+	btScalar hingeAngle = getHingeAngle();
+	m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle);
+	return m_accumulatedAngle;
+}
+void	btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
+{
+	m_accumulatedAngle  = accAngle;
+}
+
+void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info)
+{
+	//update m_accumulatedAngle
+	btScalar curHingeAngle = getHingeAngle();
+	m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle);
+
+	btHingeConstraint::getInfo1(info);
+	
+}
+
+
 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
 {
+
+
 	if (m_useSolveConstraintObsolete)
 	{
 		info->m_numConstraintRows = 0;
@@ -413,7 +481,9 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
 	}
 	// linear RHS
-    btScalar k = info->fps * info->erp;
+	btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
+
+    btScalar k = info->fps * normalErp;
 	if (!m_angularOnly)
 	{
 		for(i = 0; i < 3; i++)
@@ -510,7 +580,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 			powered = 0;
 		}
 		info->m_constraintError[srow] = btScalar(0.0f);
-		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
+		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
 		if(powered)
 		{
 			if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
@@ -606,6 +676,8 @@ void	btHingeConstraint::updateRHS(btScalar	timeStep)
 }
 
 
+
+
 btScalar btHingeConstraint::getHingeAngle()
 {
 	return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
@@ -798,7 +870,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
     for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
 
-	btScalar k = info->fps * info->erp;
+	btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp;
+	btScalar k = info->fps * normalErp;
 
 	if (!m_angularOnly)
 	{
@@ -856,7 +929,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	//    angular_velocity  = (erp*fps) * (ax1 x ax2)
 	// ax1 x ax2 is in the plane space of ax1, so we project the angular
 	// velocity to p and q to find the right hand side.
-	k = info->fps * info->erp;
+	k = info->fps * normalErp;//??
+
 	btVector3 u = ax1A.cross(ax1B);
 	info->m_constraintError[s3] = k * u.dot(p);
 	info->m_constraintError[s4] = k * u.dot(q);
@@ -901,7 +975,7 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 			powered = 0;
 		}
 		info->m_constraintError[srow] = btScalar(0.0f);
-		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
+		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
 		if(powered)
 		{
 			if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
@@ -1002,6 +1076,10 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis)
 				m_normalCFM = value;
 				m_flags |= BT_HINGE_FLAGS_CFM_NORM;
 				break;
+			case BT_CONSTRAINT_ERP:
+				m_normalERP = value;
+				m_flags |= BT_HINGE_FLAGS_ERP_NORM;
+				break;
 			default : 
 				btAssertConstrParams(0);
 		}
@@ -1032,6 +1110,10 @@ btScalar btHingeConstraint::getParam(int num, int axis) const
 				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
 				retVal = m_normalCFM;
 				break;
+			case BT_CONSTRAINT_ERP:
+				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM);
+				retVal = m_normalERP;
+				break;
 			default : 
 				btAssertConstrParams(0);
 		}

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

@@ -41,7 +41,8 @@ enum btHingeFlags
 {
 	BT_HINGE_FLAGS_CFM_STOP = 1,
 	BT_HINGE_FLAGS_ERP_STOP = 2,
-	BT_HINGE_FLAGS_CFM_NORM = 4
+	BT_HINGE_FLAGS_CFM_NORM = 4,
+	BT_HINGE_FLAGS_ERP_NORM = 8
 };
 
 
@@ -94,6 +95,7 @@ public:
 
 	int			m_flags;
 	btScalar	m_normalCFM;
+	btScalar	m_normalERP;
 	btScalar	m_stopCFM;
 	btScalar	m_stopERP;
 
@@ -217,6 +219,14 @@ public:
 
 	}
 
+    bool hasLimit() const {
+#ifdef  _BT_USE_CENTER_LIMIT_
+        return m_limit.getHalfRange() > 0;
+#else
+        return m_lowerLimit <= m_upperLimit;
+#endif
+    }
+
 	btScalar	getLowerLimit() const
 	{
 #ifdef	_BT_USE_CENTER_LIMIT_
@@ -236,6 +246,7 @@ public:
 	}
 
 
+	///The getHingeAngle gives the hinge angle in range [-PI,PI]
 	btScalar getHingeAngle();
 
 	btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
@@ -326,6 +337,43 @@ struct	btHingeConstraintDoubleData
 };
 #endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 
+///The getAccumulatedHingeAngle returns the accumulated hinge angle, taking rotation across the -PI/PI boundary into account
+ATTRIBUTE_ALIGNED16(class) btHingeAccumulatedAngleConstraint : public btHingeConstraint
+{
+protected:
+	btScalar	m_accumulatedAngle;
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
+	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false)
+	:btHingeConstraint(rbA,rbB,pivotInA,pivotInB, axisInA,axisInB, useReferenceFrameA )
+	{
+		m_accumulatedAngle=getHingeAngle();
+	}
+
+	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false)
+	:btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA)
+	{
+		m_accumulatedAngle=getHingeAngle();
+	}
+	
+	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
+	:btHingeConstraint(rbA,rbB, rbAFrame, rbBFrame, useReferenceFrameA )
+	{
+		m_accumulatedAngle=getHingeAngle();
+	}
+
+	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false)
+	:btHingeConstraint(rbA,rbAFrame, useReferenceFrameA )
+	{
+		m_accumulatedAngle=getHingeAngle();
+	}
+	btScalar getAccumulatedHingeAngle();
+	void	setAccumulatedHingeAngle(btScalar accAngle);
+	virtual void getInfo1 (btConstraintInfo1* info);
+
+};
 
 struct	btHingeConstraintFloatData
 {

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

@@ -0,0 +1,463 @@
+/*
+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 "btNNCGConstraintSolver.h"
+
+
+
+
+
+
+btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
+
+	m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
+	m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
+	m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
+	m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
+
+	m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
+	m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
+	m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
+	m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
+
+	return val;
+}
+
+btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
+{
+
+	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
+	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+
+	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+	{
+		if (1)			// uncomment this for a bit less random ((iteration & 7) == 0)
+		{
+
+			for (int j=0; j<numNonContactPool; ++j) {
+				int tmp = m_orderNonContactConstraintPool[j];
+				int swapi = btRandInt2(j+1);
+				m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
+				m_orderNonContactConstraintPool[swapi] = tmp;
+			}
+
+			//contact/friction constraints are not solved more than 
+			if (iteration< infoGlobal.m_numIterations)
+			{
+				for (int j=0; j<numConstraintPool; ++j) {
+					int tmp = m_orderTmpConstraintPool[j];
+					int swapi = btRandInt2(j+1);
+					m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
+					m_orderTmpConstraintPool[swapi] = tmp;
+				}
+
+				for (int j=0; j<numFrictionPool; ++j) {
+					int tmp = m_orderFrictionConstraintPool[j];
+					int swapi = btRandInt2(j+1);
+					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+					m_orderFrictionConstraintPool[swapi] = tmp;
+				}
+			}
+		}
+	}
+
+
+	btScalar deltaflengthsqr = 0;
+
+	if (infoGlobal.m_solverMode & SOLVER_SIMD)
+	{
+		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		{
+			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+			if (iteration < constraint.m_overrideNumSolverIterations) 
+			{
+				btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				m_deltafNC[j] = deltaf;
+				deltaflengthsqr += deltaf * deltaf;
+			}
+		}
+	} else 
+	{
+		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		{
+			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+			if (iteration < constraint.m_overrideNumSolverIterations) 
+			{
+				btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				m_deltafNC[j] = deltaf;
+				deltaflengthsqr += deltaf * deltaf;
+			}
+		}
+	}
+
+
+	if (m_onlyForNoneContact) 
+	{
+		if (iteration==0) 
+		{
+			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
+		} else {
+			// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
+			btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
+			if (beta>1) 
+			{
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
+			} else 
+			{
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+					if (iteration < constraint.m_overrideNumSolverIterations) 
+					{
+						btScalar additionaldeltaimpulse = beta * m_pNC[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+			}
+		}
+		m_deltafLengthSqrPrev = deltaflengthsqr;
+	}
+
+
+
+	if (infoGlobal.m_solverMode & SOLVER_SIMD)
+	{
+
+		if (iteration< infoGlobal.m_numIterations)
+		{
+			for (int j=0;j<numConstraints;j++)
+			{
+				if (constraints[j]->isEnabled())
+				{
+					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);
+				}
+			}
+
+			///solve all contact constraints using SIMD, if available
+			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
+			{
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
+
+				for (int c=0;c<numPoolConstraints;c++)
+				{
+					btScalar totalImpulse =0;
+
+					{
+						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
+						btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);						
+						m_deltafC[c] = deltaf;
+						deltaflengthsqr += deltaf*deltaf;
+						totalImpulse = solveManifold.m_appliedImpulse;
+					}
+					bool applyFriction = true;
+					if (applyFriction)
+					{
+						{
+
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
+
+							if (totalImpulse>btScalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								m_deltafCF[c*multiplier] = deltaf;
+								deltaflengthsqr += deltaf*deltaf;
+							} else {
+								m_deltafCF[c*multiplier] = 0;
+							}
+						}
+
+						if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
+						{
+
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
+
+							if (totalImpulse>btScalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								m_deltafCF[c*multiplier+1] = deltaf;
+								deltaflengthsqr += deltaf*deltaf;
+							} else {
+								m_deltafCF[c*multiplier+1] = 0;
+							}
+						}
+					}
+				}
+
+			}
+			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			{
+				//solve the friction constraints after all contact constraints, don't interleave them
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int j;
+
+				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);
+					btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					m_deltafC[j] = deltaf;
+					deltaflengthsqr += deltaf*deltaf;
+				}
+
+
+
+				///solve all friction constraints, using SIMD, if available
+
+				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+				for (j=0;j<numFrictionPoolConstraints;j++)
+				{
+					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+					if (totalImpulse>btScalar(0))
+					{
+						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);
+						btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						m_deltafCF[j] = deltaf;
+						deltaflengthsqr += deltaf*deltaf;
+					} else {
+						m_deltafCF[j] = 0;
+					}
+				}
+
+
+				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+				for (j=0;j<numRollingFrictionPoolConstraints;j++)
+				{
+
+					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+					if (totalImpulse>btScalar(0))
+					{
+						btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
+						if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+							rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+
+						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+						btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+						m_deltafCRF[j] = deltaf;
+						deltaflengthsqr += deltaf*deltaf;
+					} else {
+						m_deltafCRF[j] = 0;
+					}
+				}
+
+
+			}
+		}
+
+
+
+	} else
+	{
+
+		if (iteration< infoGlobal.m_numIterations)
+		{
+			for (int j=0;j<numConstraints;j++)
+			{
+				if (constraints[j]->isEnabled())
+				{
+					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);
+				}
+			}
+			///solve all contact constraints
+			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+			for (int j=0;j<numPoolConstraints;j++)
+			{
+				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+				btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+				m_deltafC[j] = deltaf;
+				deltaflengthsqr += deltaf*deltaf;
+			}
+			///solve all friction constraints
+			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+			for (int j=0;j<numFrictionPoolConstraints;j++)
+			{
+				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+				btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+				if (totalImpulse>btScalar(0))
+				{
+					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+					solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					m_deltafCF[j] = deltaf;
+					deltaflengthsqr += deltaf*deltaf;
+				} else {
+					m_deltafCF[j] = 0;
+				}
+			}
+
+			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+			for (int j=0;j<numRollingFrictionPoolConstraints;j++)
+			{
+				btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+				btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+				if (totalImpulse>btScalar(0))
+				{
+					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
+					if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+
+					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+					m_deltafCRF[j] = deltaf;
+					deltaflengthsqr += deltaf*deltaf;
+				} else {
+					m_deltafCRF[j] = 0;
+				}
+			}
+		}
+	}
+
+
+
+
+	if (!m_onlyForNoneContact) 
+	{
+		if (iteration==0) 
+		{
+			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
+			for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
+			for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j];
+			if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) 
+			{
+				for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
+			}
+		} else 
+		{
+			// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
+			btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
+			if (beta>1) {
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
+				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
+				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0;
+				if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
+					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
+				}
+			} else {
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+					if (iteration < constraint.m_overrideNumSolverIterations) {
+						btScalar additionaldeltaimpulse = beta * m_pNC[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+					if (iteration< infoGlobal.m_numIterations) {
+						btScalar additionaldeltaimpulse = beta * m_pC[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pC[j] = beta * m_pC[j] + m_deltafC[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					if (iteration< infoGlobal.m_numIterations) {
+						btScalar additionaldeltaimpulse = beta * m_pCF[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+				if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
+					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++)
+					{
+						btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+						if (iteration< infoGlobal.m_numIterations) {
+							btScalar additionaldeltaimpulse = beta * m_pCRF[j];
+							constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+							m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
+							btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+							btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+							const btSolverConstraint& c = constraint;
+							body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+							body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+						}
+					}
+				}
+			}
+		}
+		m_deltafLengthSqrPrev = deltaflengthsqr;
+	}
+
+	return deltaflengthsqr;
+}
+
+btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
+{
+	m_pNC.resizeNoInitialize(0);
+	m_pC.resizeNoInitialize(0);
+	m_pCF.resizeNoInitialize(0);
+	m_pCRF.resizeNoInitialize(0);
+
+	m_deltafNC.resizeNoInitialize(0);
+	m_deltafC.resizeNoInitialize(0);
+	m_deltafCF.resizeNoInitialize(0);
+	m_deltafCRF.resizeNoInitialize(0);
+
+	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
+}
+
+
+

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

@@ -0,0 +1,64 @@
+/*
+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_NNCG_CONSTRAINT_SOLVER_H
+#define BT_NNCG_CONSTRAINT_SOLVER_H
+
+#include "btSequentialImpulseConstraintSolver.h"
+
+ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
+{
+protected:
+
+	btScalar m_deltafLengthSqrPrev;
+
+	btAlignedObjectArray<btScalar> m_pNC;  // p for None Contact constraints
+	btAlignedObjectArray<btScalar> m_pC;   // p for Contact constraints
+	btAlignedObjectArray<btScalar> m_pCF;  // p for ContactFriction constraints
+	btAlignedObjectArray<btScalar> m_pCRF; // p for ContactRollingFriction constraints
+
+	//These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration.
+	btAlignedObjectArray<btScalar> m_deltafNC;  // deltaf for NoneContact constraints
+	btAlignedObjectArray<btScalar> m_deltafC;   // deltaf for Contact constraints
+	btAlignedObjectArray<btScalar> m_deltafCF;  // deltaf for ContactFriction constraints
+	btAlignedObjectArray<btScalar> m_deltafCRF; // deltaf for ContactRollingFriction constraints
+
+		
+protected:
+
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
+	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);
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {}
+
+	virtual btConstraintSolverType getSolverType() const
+	{
+		return BT_NNCG_SOLVER;
+	}
+
+	bool m_onlyForNoneContact;
+};
+
+
+
+
+#endif //BT_NNCG_CONSTRAINT_SOLVER_H
+

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

@@ -4,8 +4,8 @@ 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, 
+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.
@@ -13,8 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-// Modified by Lasse Oorni for Urho3D
-
 //#define COMPUTE_IMPULSE_DENOM 1
 //#define BT_ADDITIONAL_DEBUG
 
@@ -24,6 +22,8 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 
 #include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btCpuFeatureUtility.h"
+
 //#include "btJacobianEntry.h"
 #include "LinearMath/btMinMax.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
@@ -39,142 +39,262 @@ int		gNumSplitImpulseRecoveries = 0;
 
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
-btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
-:m_btSeed2(0)
+
+///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver
+///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check.
+static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
 {
+	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
+	const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
+	const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
 
+	//	const btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
+	deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel2Dotn*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;
+	}
+
+	body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+	body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+
+	return deltaImpulse;
 }
 
-btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
+
+static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
 {
+	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
+	const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
+	const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
+
+	deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel2Dotn*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
+	{
+		c.m_appliedImpulse = sum;
+	}
+	body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+	body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+
+	return deltaImpulse;
 }
 
+
+
 #ifdef USE_SIMD
 #include <emmintrin.h>
+
+
 #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
 static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
 {
 	__m128 result = _mm_mul_ps( vec0, vec1);
 	return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
 }
-#endif//USE_SIMD
+
+#if defined (BT_ALLOW_SSE4)
+#include <intrin.h>
+
+#define USE_FMA					1
+#define USE_FMA3_INSTEAD_FMA4	1
+#define USE_SSE4_DOT			1
+
+#define SSE4_DP(a, b)			_mm_dp_ps(a, b, 0x7f)
+#define SSE4_DP_FP(a, b)		_mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f))
+
+#if USE_SSE4_DOT
+#define DOT_PRODUCT(a, b)		SSE4_DP(a, b)
+#else
+#define DOT_PRODUCT(a, b)		btSimdDot3(a, b)
+#endif
+
+#if USE_FMA
+#if USE_FMA3_INSTEAD_FMA4
+// a*b + c
+#define FMADD(a, b, c)		_mm_fmadd_ps(a, b, c)
+// -(a*b) + c
+#define FMNADD(a, b, c)		_mm_fnmadd_ps(a, b, c)
+#else // USE_FMA3
+// a*b + c
+#define FMADD(a, b, c)		_mm_macc_ps(a, b, c)
+// -(a*b) + c
+#define FMNADD(a, b, c)		_mm_nmacc_ps(a, b, c)
+#endif
+#else // USE_FMA
+// c + a*b
+#define FMADD(a, b, c)		_mm_add_ps(c, _mm_mul_ps(a, b))
+// c - a*b
+#define FMNADD(a, b, c)		_mm_sub_ps(c, _mm_mul_ps(a, b))
+#endif
+#endif
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
-void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
 {
-#ifdef USE_SIMD
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
 	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
 	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
-	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
-	__m128 deltaVel1Dotn	=	_mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
-	__m128 deltaVel2Dotn	=	_mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
-	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
-	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
-	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
-	btSimdScalar resultLowerLess,resultUpperLess;
-	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
-	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
-	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
-	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
-	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
-	__m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
-	deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
-	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
-	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
-	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128,body2.internalGetInvMass().mVec128);
+	btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
+	btSimdScalar resultLowerLess, resultUpperLess;
+	resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
+	resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
+	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
+	deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
+	c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
+	__m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp);
+	deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied));
+	c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1));
+	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128);
+	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, body2.internalGetInvMass().mVec128);
 	__m128 impulseMagnitude = deltaImpulse;
-	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
-	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
-	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
-	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
+	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
+	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
+	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
+	return deltaImpulse;
+}
+
+
+// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
+static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
+{
+#if defined (BT_ALLOW_SSE4)
+	__m128 tmp					= _mm_set_ps1(c.m_jacDiagABInv);
+	__m128 deltaImpulse			= _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm);
+	const __m128 lowerLimit		= _mm_set_ps1(c.m_lowerLimit);
+	const __m128 upperLimit		= _mm_set_ps1(c.m_upperLimit);
+	const __m128 deltaVel1Dotn	= _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
+	const __m128 deltaVel2Dotn	= _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
+	deltaImpulse				= FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
+	deltaImpulse				= FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
+	tmp							= _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum
+	const __m128 maskLower		= _mm_cmpgt_ps(tmp, lowerLimit);
+	const __m128 maskUpper		= _mm_cmpgt_ps(upperLimit, tmp);
+	deltaImpulse				= _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower);
+	c.m_appliedImpulse			= _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower);
+	body1.internalGetDeltaLinearVelocity().mVec128	= FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128);
+	body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128);
+	body2.internalGetDeltaLinearVelocity().mVec128	= FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128);
+	body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128);
+	return deltaImpulse;
 #else
-	resolveSingleConstraintRowGeneric(body1,body2,c);
+	return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c);
 #endif
 }
 
-// Project Gauss Seidel or the equivalent Sequential Impulse
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
-{
-	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
-	const btScalar deltaVel1Dotn	=	c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
-	const btScalar deltaVel2Dotn	=	c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity())  + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
 
-//	const btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
-	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
-	deltaImpulse	-=	deltaVel2Dotn*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;
-	}
-
-	body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
-	body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
-}
-
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
 {
-#ifdef USE_SIMD
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
 	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
 	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
-	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
-	__m128 deltaVel1Dotn	=	_mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
-	__m128 deltaVel2Dotn	=	_mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
-	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
-	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
-	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
-	btSimdScalar resultLowerLess,resultUpperLess;
-	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
-	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
-	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
-	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
-	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
-	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
-	__m128	linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
+	btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
+	btSimdScalar resultLowerLess, resultUpperLess;
+	resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
+	resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
+	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
+	deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
+	c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
+	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128);
+	__m128	linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128);
 	__m128 impulseMagnitude = deltaImpulse;
-	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
-	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
-	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
-	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
+	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
+	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
+	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
+	return deltaImpulse;
+}
+
+
+// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
+static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
+{
+#ifdef BT_ALLOW_SSE4
+	__m128 tmp					= _mm_set_ps1(c.m_jacDiagABInv);
+	__m128 deltaImpulse			= _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm);
+	const __m128 lowerLimit		= _mm_set_ps1(c.m_lowerLimit);
+	const __m128 deltaVel1Dotn	= _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
+	const __m128 deltaVel2Dotn	= _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
+	deltaImpulse				= FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
+	deltaImpulse				= FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
+	tmp							= _mm_add_ps(c.m_appliedImpulse, deltaImpulse);
+	const __m128 mask			= _mm_cmpgt_ps(tmp, lowerLimit);
+	deltaImpulse				= _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask);
+	c.m_appliedImpulse			= _mm_blendv_ps(lowerLimit, tmp, mask);
+	body1.internalGetDeltaLinearVelocity().mVec128	= FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128);
+	body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128);
+	body2.internalGetDeltaLinearVelocity().mVec128	= FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128);
+	body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128);
+	return deltaImpulse;
 #else
-	resolveSingleConstraintRowLowerLimit(body1,body2,c);
+	return gResolveSingleConstraintRowLowerLimit_sse2(body1,body2,c);
+#endif //BT_ALLOW_SSE4
+}
+
+
+#endif //USE_SIMD
+
+
+
+btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+	return m_resolveSingleConstraintRowGeneric(body1, body2, c);
+#else
+	return resolveSingleConstraintRowGeneric(body1,body2,c);
 #endif
 }
 
-// Projected Gauss Seidel or the equivalent Sequential Impulse
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+// Project Gauss Seidel or the equivalent Sequential Impulse
+btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
-	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
-	const btScalar deltaVel1Dotn	=	c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
-	const btScalar deltaVel2Dotn	=	c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
+	return gResolveSingleConstraintRowGeneric_scalar_reference(body1, body2, c);
+}
 
-	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
-	deltaImpulse	-=	deltaVel2Dotn*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
-	{
-		c.m_appliedImpulse = sum;
-	}
-	body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
-	body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+	return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
+#else
+	return resolveSingleConstraintRowLowerLimit(body1,body2,c);
+#endif
+}
+
+
+btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+	return gResolveSingleConstraintRowLowerLimit_scalar_reference(body1,body2,c);
 }
 
 
@@ -243,6 +363,63 @@ void	btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
 }
 
 
+ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
+	 : m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference),
+	 m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference),
+	 m_btSeed2(0)
+ {
+
+#ifdef USE_SIMD
+	 m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
+	 m_resolveSingleConstraintRowLowerLimit=gResolveSingleConstraintRowLowerLimit_sse2;
+#endif //USE_SIMD
+
+#ifdef BT_ALLOW_SSE4
+	 int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
+	 if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1))
+	 {
+		m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3;
+		m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
+	 }
+#endif//BT_ALLOW_SSE4
+
+ }
+
+ btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
+ {
+ }
+
+ btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()
+ {
+	 return gResolveSingleConstraintRowGeneric_scalar_reference;
+ }
+
+ btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()
+ {
+	 return gResolveSingleConstraintRowLowerLimit_scalar_reference;
+ }
+
+
+#ifdef USE_SIMD
+ btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric()
+ {
+	 return gResolveSingleConstraintRowGeneric_sse2;
+ }
+ btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit()
+ {
+	 return gResolveSingleConstraintRowLowerLimit_sse2;
+ }
+#ifdef BT_ALLOW_SSE4
+ btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric()
+ {
+	 return gResolveSingleConstraintRowGeneric_sse4_1_fma3;
+ }
+ btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit()
+ {
+	 return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
+ }
+#endif //BT_ALLOW_SSE4
+#endif //USE_SIMD
 
 unsigned long btSequentialImpulseConstraintSolver::btRand2()
 {
@@ -303,7 +480,7 @@ void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
 		solverBody->m_angularVelocity = rb->getAngularVelocity();
 		solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep;
 		solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;
-		
+
 	} else
 	{
 		solverBody->m_worldTransform.setIdentity();
@@ -335,7 +512,7 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel,
 
 void	btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
 {
-	
+
 
 	if (colObj && colObj->hasAnisotropicFriction(frictionMode))
 	{
@@ -356,7 +533,7 @@ void	btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb
 void btSequentialImpulseConstraintSolver::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, btScalar desiredVelocity, btScalar cfmSlip)
 {
 
-	
+
 	btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
 	btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
 
@@ -417,26 +594,26 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 	}
 
 	{
-		
+
 
 		btScalar rel_vel;
-		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse: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)) 
+		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;
 
 //		btScalar positionalError = 0.f;
 
-        // Urho3D: possible friction fix from https://github.com/bulletphysics/bullet3/commit/907ac49892ede3f4a3295f7cbd96759107e5fc0e
 		btScalar velocityError =  desiredVelocity - rel_vel;
-		btScalar	velocityImpulse = velocityError * btScalar(solverConstraint.m_jacDiagABInv);
+		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
 		solverConstraint.m_rhs = velocityImpulse;
+		solverConstraint.m_rhsPenetration = 0.f;
 		solverConstraint.m_cfm = cfmSlip;
 		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
 		solverConstraint.m_upperLimit = solverConstraint.m_friction;
-		
+
 	}
 }
 
@@ -444,7 +621,7 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(c
 {
 	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
-	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, 
+	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
 							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
@@ -452,7 +629,7 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(c
 
 void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int  solverBodyIdB,
 									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
-									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
+									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
 									btScalar desiredVelocity, btScalar cfmSlip)
 
 {
@@ -498,26 +675,25 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolv
 	}
 
 	{
-		
+
 
 		btScalar rel_vel;
-		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse: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)) 
+		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;
 
 //		btScalar positionalError = 0.f;
 
-        // Urho3D: possible friction fix from https://github.com/bulletphysics/bullet3/commit/907ac49892ede3f4a3295f7cbd96759107e5fc0e
-		btScalar velocityError =  desiredVelocity - rel_vel;
-		btScalar	velocityImpulse = velocityError * btScalar(solverConstraint.m_jacDiagABInv);
+		btSimdScalar velocityError =  desiredVelocity - rel_vel;
+		btSimdScalar	velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
 		solverConstraint.m_rhs = velocityImpulse;
 		solverConstraint.m_cfm = cfmSlip;
 		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
 		solverConstraint.m_upperLimit = solverConstraint.m_friction;
-		
+
 	}
 }
 
@@ -532,7 +708,7 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addRollingFrictionConst
 {
 	btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
-	setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, 
+	setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
 							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
@@ -560,7 +736,7 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 			body.setCompanionId(solverBodyIdA);
 		} else
 		{
-			
+
 			if (m_fixedBodyId<0)
 			{
 				m_fixedBodyId = m_tmpSolverBodyPool.size();
@@ -578,15 +754,15 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 #include <stdio.h>
 
 
-void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, 
+void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
 																 int solverBodyIdA, int solverBodyIdB,
 																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
 																 btScalar& relaxation,
 																 const btVector3& rel_pos1, const btVector3& rel_pos2)
 {
-			
-			const btVector3& pos1 = cp.getPositionWorldOnA();
-			const btVector3& pos2 = cp.getPositionWorldOnB();
+
+		//	const btVector3& pos1 = cp.getPositionWorldOnA();
+		//	const btVector3& pos2 = cp.getPositionWorldOnB();
 
 			btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
 			btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
@@ -594,23 +770,23 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 			btRigidBody* rb0 = bodyA->m_originalBody;
 			btRigidBody* rb1 = bodyB->m_originalBody;
 
-//			btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
+//			btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
 //			btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
-			//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
+			//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
 			//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 
 			relaxation = 1.f;
 
 			btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
 			solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
-			btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
+			btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
 			solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
 
 				{
 #ifdef COMPUTE_IMPULSE_DENOM
 					btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
 					btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
-#else							
+#else
 					btVector3 vec;
 					btScalar denom0 = 0.f;
 					btScalar denom1 = 0.f;
@@ -624,7 +800,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 						vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
 						denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
 					}
-#endif //COMPUTE_IMPULSE_DENOM		
+#endif //COMPUTE_IMPULSE_DENOM
 
 					btScalar denom = relaxation/(denom0+denom1);
 					solverConstraint.m_jacDiagABInv = denom;
@@ -662,11 +838,11 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 					btVector3 vel  = vel1 - vel2;
 					btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
 
-					
+
 
 					solverConstraint.m_friction = cp.m_combinedFriction;
 
-				
+
 					restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
 					if (restitution <= btScalar(0.))
 					{
@@ -696,17 +872,17 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 					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) 
+
+					btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA)
 						+ solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA);
-					btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB) 
+					btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB)
 						+ solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB);
 					btScalar rel_vel = vel1Dotn+vel2Dotn;
 
 					btScalar positionalError = 0.f;
 					btScalar	velocityError = restitution - rel_vel;// * damping;
-					
+
 
 					btScalar erp = infoGlobal.m_erp2;
 					if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
@@ -751,7 +927,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 
 
 
-void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, 
+void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
 																		int solverBodyIdA, int solverBodyIdB,
 																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
 {
@@ -816,7 +992,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 
 	///avoid collision response between two static objects
-	if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
+	if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
 		return;
 
 	int rollingFriction=1;
@@ -830,7 +1006,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			btVector3 rel_pos1;
 			btVector3 rel_pos2;
 			btScalar relaxation;
-			
+
 
 			int frictionIndex = m_tmpSolverContactConstraintPool.size();
 			btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
@@ -844,7 +1020,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			const btVector3& pos1 = cp.getPositionWorldOnA();
 			const btVector3& pos2 = cp.getPositionWorldOnB();
 
-			rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
+			rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
 			rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
 
 			btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
@@ -852,13 +1028,13 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 			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();
@@ -899,21 +1075,21 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 						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);
-		
+
 				}
 			}
 
 			///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 
+			///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, 
+			///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
@@ -969,9 +1145,9 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 			}
 			setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
-		
 
-			
+
+
 
 		}
 	}
@@ -1011,7 +1187,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 				bool found=false;
 				for (int b=0;b<numBodies;b++)
 				{
-                
+
 					if (&constraint->getRigidBodyA()==bodies[b])
 					{
 						found = true;
@@ -1043,7 +1219,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
             bool found=false;
             for (int b=0;b<numBodies;b++)
             {
-                
+
                 if (manifoldPtr[i]->getBody0()==bodies[b])
                 {
                     found = true;
@@ -1067,8 +1243,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
         }
     }
 #endif //BT_ADDITIONAL_DEBUG
-	
-	
+
+
 	for (int i = 0; i < numBodies; i++)
 	{
 		bodies[i]->setCompanionId(-1);
@@ -1099,7 +1275,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 			}
 		}
 	}
-	
+
 	if (1)
 	{
 		int j;
@@ -1119,7 +1295,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 			int totalNumRows = 0;
 			int i;
-			
+
 			m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
 			//calculate the total number of contraint rows
 			for (i=0;i<numConstraints;i++)
@@ -1149,14 +1325,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 			}
 			m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
 
-			
+
 			///setup the btSolverConstraints
 			int currentRow = 0;
 
 			for (i=0;i<numConstraints;i++)
 			{
 				const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
-				
+
 				if (info1.m_numConstraintRows)
 				{
 					btAssert(currentRow<totalNumRows);
@@ -1264,7 +1440,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 						}
 
 
-						
+
 						{
 							btScalar rel_vel;
 							btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
@@ -1272,11 +1448,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 							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) 
+
+							btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
 												+ solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
-							
-							btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) 
+
+							btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
 																+ solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
 
 							rel_vel = vel1Dotn+vel2Dotn;
@@ -1342,7 +1518,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
 	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
 	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
-	
+
 	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
 	{
 		if (1)			// uncomment this for a bit less random ((iteration & 7) == 0)
@@ -1355,7 +1531,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 				m_orderNonContactConstraintPool[swapi] = tmp;
 			}
 
-			//contact/friction constraints are not solved more than 
+			//contact/friction constraints are not solved more than
 			if (iteration< infoGlobal.m_numIterations)
 			{
 				for (int j=0; j<numConstraintPool; ++j) {
@@ -1434,7 +1610,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 						{
 
 							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
-				
+
 							if (totalImpulse>btScalar(0))
 							{
 								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
@@ -1456,12 +1632,11 @@ 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);
-					resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 
 				}
-		
-				
+
+
 
 				///solve all friction constraints, using SIMD, if available
 
@@ -1476,12 +1651,11 @@ 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);
-						resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 					}
 				}
 
-				
+
 				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
 				for (j=0;j<numRollingFrictionPoolConstraints;j++)
 				{
@@ -1500,9 +1674,9 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
 					}
 				}
-				
 
-			}			
+
+			}
 		}
 	} else
 	{
@@ -1626,10 +1800,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
 
 		for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
 		//for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
-		{			
+		{
 			solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
 		}
-		
+
 	}
 	return 0.f;
 }
@@ -1671,14 +1845,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 			fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
 			fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
 			fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
-			
+
 		}
 
 		constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
-
-		// Urho3D: if constraint has infinity breaking threshold, do not break no matter what
-		btScalar breakingThreshold = constr->getBreakingImpulseThreshold();
-		if (breakingThreshold < SIMD_INFINITY && btFabs(solverConstr.m_appliedImpulse)>=breakingThreshold)
+		if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
 		{
 			constr->setEnabled(false);
 		}
@@ -1695,7 +1866,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 				m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
 			else
 				m_tmpSolverBodyPool[i].writebackVelocity();
-			
+
 			m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
 				m_tmpSolverBodyPool[i].m_linearVelocity+
 				m_tmpSolverBodyPool[i].m_externalForceImpulse);
@@ -1728,13 +1899,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
 
 	BT_PROFILE("solveGroup");
 	//you need to provide at least some bodies
-	
+
 	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
 
 	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
 
 	solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
-	
+
 	return 0.f;
 }
 
@@ -1742,5 +1913,3 @@ void	btSequentialImpulseConstraintSolver::reset()
 {
 	m_btSeed2 = 0;
 }
-
-

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

@@ -27,6 +27,8 @@ class btCollisionObject;
 #include "../../BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 #include "../../BulletDynamics/ConstraintSolver/btConstraintSolver.h"
 
+typedef btSimdScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
+
 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
 ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
@@ -43,6 +45,10 @@ protected:
 	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
 	int							m_maxOverrideNumSolverIterations;
 	int m_fixedBodyId;
+
+	btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
+	btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
+
 	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, 
@@ -88,13 +94,10 @@ protected:
 	int		getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
 	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
 
-	void	resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-
-	void	resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	
-	void	resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	
-	void	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btSimdScalar	resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btSimdScalar	resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btSimdScalar	resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btSimdScalar	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
 		
 protected:
 	
@@ -115,9 +118,7 @@ public:
 	virtual ~btSequentialImpulseConstraintSolver();
 
 	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-	
-
-	
+		
 	///clear internal cached data and reset random seed
 	virtual	void	reset();
 	
@@ -139,6 +140,33 @@ public:
 	{
 		return BT_SEQUENTIAL_IMPULSE_SOLVER;
 	}
+
+	btSingleConstraintRowSolver	getActiveConstraintRowSolverGeneric()
+	{
+		return m_resolveSingleConstraintRowGeneric;
+	}
+	void setConstraintRowSolverGeneric(btSingleConstraintRowSolver rowSolver)
+	{
+		m_resolveSingleConstraintRowGeneric = rowSolver;
+	}
+	btSingleConstraintRowSolver	getActiveConstraintRowSolverLowerLimit()
+	{
+		return m_resolveSingleConstraintRowLowerLimit;
+	}
+	void setConstraintRowSolverLowerLimit(btSingleConstraintRowSolver rowSolver)
+	{
+		m_resolveSingleConstraintRowLowerLimit = rowSolver;
+	}
+
+	///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
+	btSingleConstraintRowSolver	getScalarConstraintRowSolverGeneric();
+	btSingleConstraintRowSolver	getSSE2ConstraintRowSolverGeneric();
+	btSingleConstraintRowSolver	getSSE4_1ConstraintRowSolverGeneric();
+
+	///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4
+	btSingleConstraintRowSolver	getScalarConstraintRowSolverLowerLimit();
+	btSingleConstraintRowSolver	getSSE2ConstraintRowSolverLowerLimit();
+	btSingleConstraintRowSolver	getSSE4_1ConstraintRowSolverLowerLimit();
 };
 
 

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

@@ -24,7 +24,7 @@ subject to the following restrictions:
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
 :btTypedObject(type),
 m_userConstraintType(-1),
-m_userConstraintId(-1),
+m_userConstraintPtr((void*)-1),
 m_breakingImpulseThreshold(SIMD_INFINITY),
 m_isEnabled(true),
 m_needsFeedback(false),
@@ -41,7 +41,7 @@ m_jointFeedback(0)
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
 :btTypedObject(type),
 m_userConstraintType(-1),
-m_userConstraintId(-1),
+m_userConstraintPtr((void*)-1),
 m_breakingImpulseThreshold(SIMD_INFINITY),
 m_isEnabled(true),
 m_needsFeedback(false),

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

@@ -44,6 +44,7 @@ enum btTypedConstraintType
 	D6_SPRING_CONSTRAINT_TYPE,
 	GEAR_CONSTRAINT_TYPE,
 	FIXED_CONSTRAINT_TYPE,
+	D6_SPRING_2_CONSTRAINT_TYPE,
 	MAX_CONSTRAINT_TYPE
 };
 

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/Bullet-C-API.cpp

@@ -1,405 +0,0 @@
-/*
-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.
-*/
-
-/*
-	Draft high-level generic physics C-API. For low-level access, use the physics SDK native API's.
-	Work in progress, functionality will be added on demand.
-
-	If possible, use the richer Bullet C++ API, by including <src/btBulletDynamicsCommon.h>
-*/
-
-#include "Bullet-C-Api.h"
-#include "btBulletDynamicsCommon.h"
-#include "LinearMath/btAlignedAllocator.h"
-
-
-
-#include "LinearMath/btVector3.h"
-#include "LinearMath/btScalar.h"	
-#include "LinearMath/btMatrix3x3.h"
-#include "LinearMath/btTransform.h"
-#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
-#include "BulletCollision/CollisionShapes/btTriangleShape.h"
-
-#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
-#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
-#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
-#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
-#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
-#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
-#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
-
-
-/*
-	Create and Delete a Physics SDK	
-*/
-
-struct	btPhysicsSdk
-{
-
-//	btDispatcher*				m_dispatcher;
-//	btOverlappingPairCache*		m_pairCache;
-//	btConstraintSolver*			m_constraintSolver
-
-	btVector3	m_worldAabbMin;
-	btVector3	m_worldAabbMax;
-
-
-	//todo: version, hardware/optimization settings etc?
-	btPhysicsSdk()
-		:m_worldAabbMin(-1000,-1000,-1000),
-		m_worldAabbMax(1000,1000,1000)
-	{
-
-	}
-
-	
-};
-
-plPhysicsSdkHandle	plNewBulletSdk()
-{
-	void* mem = btAlignedAlloc(sizeof(btPhysicsSdk),16);
-	return (plPhysicsSdkHandle)new (mem)btPhysicsSdk;
-}
-
-void		plDeletePhysicsSdk(plPhysicsSdkHandle	physicsSdk)
-{
-	btPhysicsSdk* phys = reinterpret_cast<btPhysicsSdk*>(physicsSdk);
-	btAlignedFree(phys);	
-}
-
-
-/* Dynamics World */
-plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle)
-{
-	btPhysicsSdk* physicsSdk = reinterpret_cast<btPhysicsSdk*>(physicsSdkHandle);
-	void* mem = btAlignedAlloc(sizeof(btDefaultCollisionConfiguration),16);
-	btDefaultCollisionConfiguration* collisionConfiguration = new (mem)btDefaultCollisionConfiguration();
-	mem = btAlignedAlloc(sizeof(btCollisionDispatcher),16);
-	btDispatcher*				dispatcher = new (mem)btCollisionDispatcher(collisionConfiguration);
-	mem = btAlignedAlloc(sizeof(btAxisSweep3),16);
-	btBroadphaseInterface*		pairCache = new (mem)btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax);
-	mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
-	btConstraintSolver*			constraintSolver = new(mem) btSequentialImpulseConstraintSolver();
-
-	mem = btAlignedAlloc(sizeof(btDiscreteDynamicsWorld),16);
-	return (plDynamicsWorldHandle) new (mem)btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration);
-}
-void           plDeleteDynamicsWorld(plDynamicsWorldHandle world)
-{
-	//todo: also clean up the other allocations, axisSweep, pairCache,dispatcher,constraintSolver,collisionConfiguration
-	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	btAlignedFree(dynamicsWorld);
-}
-
-void	plStepSimulation(plDynamicsWorldHandle world,	plReal	timeStep)
-{
-	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	btAssert(dynamicsWorld);
-	dynamicsWorld->stepSimulation(timeStep);
-}
-
-void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
-{
-	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	btAssert(dynamicsWorld);
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-
-	dynamicsWorld->addRigidBody(body);
-}
-
-void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
-{
-	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	btAssert(dynamicsWorld);
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-
-	dynamicsWorld->removeRigidBody(body);
-}
-
-/* Rigid Body  */
-
-plRigidBodyHandle plCreateRigidBody(	void* user_data,  float mass, plCollisionShapeHandle cshape )
-{
-	btTransform trans;
-	trans.setIdentity();
-	btVector3 localInertia(0,0,0);
-	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
-	btAssert(shape);
-	if (mass)
-	{
-		shape->calculateLocalInertia(mass,localInertia);
-	}
-	void* mem = btAlignedAlloc(sizeof(btRigidBody),16);
-	btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia);
-	btRigidBody* body = new (mem)btRigidBody(rbci);
-	body->setWorldTransform(trans);
-	body->setUserPointer(user_data);
-	return (plRigidBodyHandle) body;
-}
-
-void plDeleteRigidBody(plRigidBodyHandle cbody)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody);
-	btAssert(body);
-	btAlignedFree( body);
-}
-
-
-/* Collision Shape definition */
-
-plCollisionShapeHandle plNewSphereShape(plReal radius)
-{
-	void* mem = btAlignedAlloc(sizeof(btSphereShape),16);
-	return (plCollisionShapeHandle) new (mem)btSphereShape(radius);
-	
-}
-	
-plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z)
-{
-	void* mem = btAlignedAlloc(sizeof(btBoxShape),16);
-	return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z));
-}
-
-plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height)
-{
-	//capsule is convex hull of 2 spheres, so use btMultiSphereShape
-	
-	const int numSpheres = 2;
-	btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)};
-	btScalar radi[numSpheres] = {radius,radius};
-	void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16);
-	return (plCollisionShapeHandle) new (mem)btMultiSphereShape(positions,radi,numSpheres);
-}
-plCollisionShapeHandle plNewConeShape(plReal radius, plReal height)
-{
-	void* mem = btAlignedAlloc(sizeof(btConeShape),16);
-	return (plCollisionShapeHandle) new (mem)btConeShape(radius,height);
-}
-
-plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height)
-{
-	void* mem = btAlignedAlloc(sizeof(btCylinderShape),16);
-	return (plCollisionShapeHandle) new (mem)btCylinderShape(btVector3(radius,height,radius));
-}
-
-/* Convex Meshes */
-plCollisionShapeHandle plNewConvexHullShape()
-{
-	void* mem = btAlignedAlloc(sizeof(btConvexHullShape),16);
-	return (plCollisionShapeHandle) new (mem)btConvexHullShape();
-}
-
-
-/* Concave static triangle meshes */
-plMeshInterfaceHandle		   plNewMeshInterface()
-{
-	return 0;
-}
-
-plCollisionShapeHandle plNewCompoundShape()
-{
-	void* mem = btAlignedAlloc(sizeof(btCompoundShape),16);
-	return (plCollisionShapeHandle) new (mem)btCompoundShape();
-}
-
-void	plAddChildShape(plCollisionShapeHandle compoundShapeHandle,plCollisionShapeHandle childShapeHandle, plVector3 childPos,plQuaternion childOrn)
-{
-	btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>(compoundShapeHandle);
-	btAssert(colShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE);
-	btCompoundShape* compoundShape = reinterpret_cast<btCompoundShape*>(colShape);
-	btCollisionShape* childShape = reinterpret_cast<btCollisionShape*>(childShapeHandle);
-	btTransform	localTrans;
-	localTrans.setIdentity();
-	localTrans.setOrigin(btVector3(childPos[0],childPos[1],childPos[2]));
-	localTrans.setRotation(btQuaternion(childOrn[0],childOrn[1],childOrn[2],childOrn[3]));
-	compoundShape->addChildShape(localTrans,childShape);
-}
-
-void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient)
-{
-	btQuaternion orn;
-	orn.setEuler(yaw,pitch,roll);
-	orient[0] = orn.getX();
-	orient[1] = orn.getY();
-	orient[2] = orn.getZ();
-	orient[3] = orn.getW();
-
-}
-
-
-//	extern  void		plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
-//	extern  plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
-
-
-void		plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z)
-{
-	btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>( cshape);
-	(void)colShape;
-	btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE);
-	btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape);
-	convexHullShape->addPoint(btVector3(x,y,z));
-
-}
-
-void plDeleteShape(plCollisionShapeHandle cshape)
-{
-	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
-	btAssert(shape);
-	btAlignedFree(shape);
-}
-void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling)
-{
-	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
-	btAssert(shape);
-	btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]);
-	shape->setLocalScaling(scaling);	
-}
-
-
-
-void plSetPosition(plRigidBodyHandle object, const plVector3 position)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	btVector3 pos(position[0],position[1],position[2]);
-	btTransform worldTrans = body->getWorldTransform();
-	worldTrans.setOrigin(pos);
-	body->setWorldTransform(worldTrans);
-}
-
-void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]);
-	btTransform worldTrans = body->getWorldTransform();
-	worldTrans.setRotation(orn);
-	body->setWorldTransform(worldTrans);
-}
-
-void	plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	btTransform& worldTrans = body->getWorldTransform();
-	worldTrans.setFromOpenGLMatrix(matrix);
-}
-
-void	plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	body->getWorldTransform().getOpenGLMatrix(matrix);
-
-}
-
-void	plGetPosition(plRigidBodyHandle object,plVector3 position)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	const btVector3& pos = body->getWorldTransform().getOrigin();
-	position[0] = pos.getX();
-	position[1] = pos.getY();
-	position[2] = pos.getZ();
-}
-
-void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	const btQuaternion& orn = body->getWorldTransform().getRotation();
-	orientation[0] = orn.getX();
-	orientation[1] = orn.getY();
-	orientation[2] = orn.getZ();
-	orientation[3] = orn.getW();
-}
-
-
-
-//plRigidBodyHandle plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
-
-//	extern  plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
-
-double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3])
-{
-	btVector3 vp(p1[0], p1[1], p1[2]);
-	btTriangleShape trishapeA(vp, 
-				  btVector3(p2[0], p2[1], p2[2]), 
-				  btVector3(p3[0], p3[1], p3[2]));
-	trishapeA.setMargin(0.000001f);
-	btVector3 vq(q1[0], q1[1], q1[2]);
-	btTriangleShape trishapeB(vq, 
-				  btVector3(q2[0], q2[1], q2[2]), 
-				  btVector3(q3[0], q3[1], q3[2]));
-	trishapeB.setMargin(0.000001f);
-	
-	// btVoronoiSimplexSolver sGjkSimplexSolver;
-	// btGjkEpaPenetrationDepthSolver penSolverPtr;	
-	
-	static btSimplexSolverInterface sGjkSimplexSolver;
-	sGjkSimplexSolver.reset();
-	
-	static btGjkEpaPenetrationDepthSolver Solver0;
-	static btMinkowskiPenetrationDepthSolver Solver1;
-		
-	btConvexPenetrationDepthSolver* Solver = NULL;
-	
-	Solver = &Solver1;	
-		
-	btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver);
-	
-	convexConvex.m_catchDegeneracies = 1;
-	
-	// btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0);
-	
-	btPointCollector gjkOutput;
-	btGjkPairDetector::ClosestPointInput input;
-	
-		
-	btTransform tr;
-	tr.setIdentity();
-	
-	input.m_transformA = tr;
-	input.m_transformB = tr;
-	
-	convexConvex.getClosestPoints(input, gjkOutput, 0);
-	
-	
-	if (gjkOutput.m_hasResult)
-	{
-		
-		pb[0] = pa[0] = gjkOutput.m_pointInWorld[0];
-		pb[1] = pa[1] = gjkOutput.m_pointInWorld[1];
-		pb[2] = pa[2] = gjkOutput.m_pointInWorld[2];
-
-		pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance;
-		pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance;
-		pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance;
-		
-		normal[0] = gjkOutput.m_normalOnBInWorld[0];
-		normal[1] = gjkOutput.m_normalOnBInWorld[1];
-		normal[2] = gjkOutput.m_normalOnBInWorld[2];
-
-		return gjkOutput.m_distance;
-	}
-	return -1.0f;	
-}
-

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

@@ -4,8 +4,8 @@ Copyright (c) 2003-2009 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, 
+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.
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-// Modified by Lasse Oorni for Urho3D
 
 #include "btDiscreteDynamicsWorld.h"
 
@@ -35,6 +34,7 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
 
@@ -59,7 +59,7 @@ int firstHit=startHit;
 SIMD_FORCE_INLINE	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
 {
 	int islandId;
-	
+
 	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
 	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
 	islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
@@ -89,7 +89,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 	int						m_numConstraints;
 	btIDebugDraw*			m_debugDrawer;
 	btDispatcher*			m_dispatcher;
-	
+
 	btAlignedObjectArray<btCollisionObject*> m_bodies;
 	btAlignedObjectArray<btPersistentManifold*> m_manifolds;
 	btAlignedObjectArray<btTypedConstraint*> m_constraints;
@@ -128,7 +128,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 		m_constraints.resize (0);
 	}
 
-	
+
 	virtual	void	processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
 	{
 		if (islandId<0)
@@ -141,7 +141,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 			btTypedConstraint** startConstraint = 0;
 			int numCurConstraints = 0;
 			int i;
-			
+
 			//find the first constraint for this island
 			for (i=0;i<m_numConstraints;i++)
 			{
@@ -165,7 +165,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 				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++)
@@ -188,7 +188,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 		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;
-			
+
 		m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_dispatcher);
 		m_bodies.resize(0);
 		m_manifolds.resize(0);
@@ -207,10 +207,10 @@ m_solverIslandCallback ( NULL ),
 m_constraintSolver(constraintSolver),
 m_gravity(0,-10,0),
 m_localTime(0),
+m_fixedTimeStep(0),
 m_synchronizeAllMotionStates(false),
 m_applySpeculativeContactRestitution(false),
 m_profileTimings(0),
-m_fixedTimeStep(0),
 m_latencyMotionStateInterpolation(true)
 
 {
@@ -318,6 +318,9 @@ void	btDiscreteDynamicsWorld::debugDrawWorld()
 			}
 		}
 	}
+    if (getDebugDrawer())
+        getDebugDrawer()->flushLines();
+
 }
 
 void	btDiscreteDynamicsWorld::clearForces()
@@ -330,7 +333,7 @@ void	btDiscreteDynamicsWorld::clearForces()
 		//it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
 		body->clearForces();
 	}
-}	
+}
 
 ///apply gravity, call this once per timestep
 void	btDiscreteDynamicsWorld::applyGravity()
@@ -444,13 +447,14 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 
 		saveKinematicState(fixedTimeStep*clampedSimulationSteps);
 
+		applyGravity();
+
+
+
 		for (int i=0;i<clampedSimulationSteps;i++)
 		{
-            // Urho3D: apply gravity and clear forces on each substep
-            applyGravity();
 			internalSingleStepSimulation(fixedTimeStep);
 			synchronizeMotionStates();
-            clearForces();
 		}
 
 	} else
@@ -458,21 +462,23 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 		synchronizeMotionStates();
 	}
 
+	clearForces();
+
 #ifndef BT_NO_PROFILE
 	CProfileManager::Increment_Frame_Counter();
 #endif //BT_NO_PROFILE
-	
+
 	return numSimulationSubSteps;
 }
 
 void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
-	
+
 	BT_PROFILE("internalSingleStepSimulation");
 
 	if(0 != m_internalPreTickCallback) {
 		(*m_internalPreTickCallback)(this, timeStep);
-	}	
+	}
 
 	///apply gravity, predict motion
 	predictUnconstraintMotion(timeStep);
@@ -485,20 +491,20 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 
 
     createPredictiveContacts(timeStep);
-    
+
 	///perform collision detection
 	performDiscreteCollisionDetection();
 
 	calculateSimulationIslands();
 
-	
+
 	getSolverInfo().m_timeStep = timeStep;
-	
+
 
 
 	///solve contact and other joint constraints
 	solveConstraints(getSolverInfo());
-	
+
 	///CallbackTriggers();
 
 	///integrate transforms
@@ -507,12 +513,12 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 
 	///update vehicle simulation
 	updateActions(timeStep);
-	
+
 	updateActivationState( timeStep );
 
 	if(0 != m_internalTickCallback) {
 		(*m_internalTickCallback)(this, timeStep);
-	}	
+	}
 }
 
 void	btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
@@ -604,14 +610,14 @@ void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short
 void	btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
 {
 	BT_PROFILE("updateActions");
-	
+
 	for ( int i=0;i<m_actions.size();i++)
 	{
 		m_actions[i]->updateAction( this, timeStep);
 	}
 }
-	
-	
+
+
 void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 {
 	BT_PROFILE("updateActivationState");
@@ -632,7 +638,7 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 				{
 					if (body->getActivationState() == ACTIVE_TAG)
 						body->setActivationState( WANTS_DEACTIVATION );
-					if (body->getActivationState() == ISLAND_SLEEPING) 
+					if (body->getActivationState() == ISLAND_SLEEPING)
 					{
 						body->setAngularVelocity(btVector3(0,0,0));
 						body->setLinearVelocity(btVector3(0,0,0));
@@ -651,6 +657,9 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 void	btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
 {
 	m_constraints.push_back(constraint);
+    //Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?)
+    btAssert(&constraint->getRigidBodyA()!=&constraint->getRigidBodyB());
+    
 	if (disableCollisionsBetweenLinkedBodies)
 	{
 		constraint->getRigidBodyA().addConstraintRef(constraint);
@@ -702,25 +711,25 @@ void	btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
 void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
 	BT_PROFILE("solveConstraints");
-	
+
 	m_sortedConstraints.resize( m_constraints.size());
-	int i; 
+	int i;
 	for (i=0;i<getNumConstraints();i++)
 	{
 		m_sortedConstraints[i] = m_constraints[i];
 	}
 
 //	btAssert(0);
-		
-	
+
+
 
 	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
-	
+
 	btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
-	
+
 	m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer());
 	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
-	
+
 	/// solve all the constraints for this island
 	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback);
 
@@ -741,10 +750,10 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
         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())))
             {
@@ -752,7 +761,7 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
             }
         }
     }
-    
+
 	{
 		int i;
 		int numConstraints = int(m_constraints.size());
@@ -776,7 +785,7 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 	//Store the island id in each body
 	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
 
-	
+
 }
 
 
@@ -792,7 +801,7 @@ public:
 	btDispatcher* m_dispatcher;
 
 public:
-	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
+	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
 	  btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
 		m_me(me),
 		m_allowedPenetration(0.0f),
@@ -872,7 +881,7 @@ int gNumClampedCcdMotions=0;
 void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 {
 	BT_PROFILE("createPredictiveContacts");
-	
+
 	{
 		BT_PROFILE("release predictive contact manifolds");
 
@@ -894,7 +903,7 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 		{
 
 			body->predictIntegratedTransform(timeStep, predictedTrans);
-			
+
 			btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
 
 			if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
@@ -908,7 +917,7 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 					{
 					public:
 
-						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
+						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
 						  btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
 						{
 						}
@@ -938,14 +947,14 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 					convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
 					if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
 					{
-					
+
 						btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction;
 						btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld);
 
-						
+
 						btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject);
 						m_predictiveManifolds.push_back(manifold);
-						
+
 						btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec;
 						btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB;
 
@@ -978,10 +987,10 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 		{
 
 			body->predictIntegratedTransform(timeStep, predictedTrans);
-			
+
 			btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
 
-			
+
 
 			if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
 			{
@@ -994,7 +1003,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 					{
 					public:
 
-						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
+						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
 						  btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
 						{
 						}
@@ -1024,7 +1033,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 					convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
 					if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
 					{
-						
+
 						//printf("clamped integration to hit fraction = %f\n",fraction);
 						body->setHitFraction(sweepResults.m_closestHitFraction);
 						body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
@@ -1049,13 +1058,13 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 							printf("sm2=%f\n",sm2);
 						}
 #else
-						
+
 						//don't apply the collision response right now, it will happen next frame
 						//if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution.
 						//btScalar appliedImpulse = 0.f;
 						//btScalar depth = 0.f;
 						//appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
-						
+
 
 #endif
 
@@ -1063,10 +1072,10 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 					}
 				}
 			}
-			
+
 
 			body->proceedToTransform( predictedTrans);
-		
+
 		}
 
 	}
@@ -1080,7 +1089,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 			btPersistentManifold* manifold = m_predictiveManifolds[i];
 			btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0());
 			btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1());
-			
+
 			for (int p=0;p<manifold->getNumContacts();p++)
 			{
 				const btManifoldPoint& pt = manifold->getContactPoint(p);
@@ -1090,11 +1099,11 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 				//if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
 				{
 					btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution;
-				
+
 					const btVector3& pos1 = pt.getPositionWorldOnA();
 					const btVector3& pos2 = pt.getPositionWorldOnB();
 
-					btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin(); 
+					btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin();
 					btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin();
 
 					if (body0)
@@ -1105,7 +1114,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 			}
 		}
 	}
-	
+
 }
 
 
@@ -1144,7 +1153,7 @@ void	btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
 
 
 
-	
+
 
 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 {
@@ -1164,12 +1173,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 				btTransform tr;
 				tr.setIdentity();
 				btVector3 pivot = p2pC->getPivotInA();
-				pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; 
+				pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
 				tr.setOrigin(pivot);
 				getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				// that ideally should draw the same frame	
+				// that ideally should draw the same frame
 				pivot = p2pC->getPivotInB();
-				pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; 
+				pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
 				tr.setOrigin(pivot);
 				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
 			}
@@ -1188,13 +1197,13 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 					break;
 				}
 				bool drawSect = true;
-				if(minAng > maxAng)
+				if(!pHinge->hasLimit())
 				{
 					minAng = btScalar(0.f);
 					maxAng = SIMD_2_PI;
 					drawSect = false;
 				}
-				if(drawLimits) 
+				if(drawLimits)
 				{
 					btVector3& center = tr.getOrigin();
 					btVector3 normal = tr.getBasis().getColumn(2);
@@ -1229,7 +1238,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 							getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
 
 						pPrev = pCur;
-					}						
+					}
 					btScalar tws = pCT->getTwistSpan();
 					btScalar twa = pCT->getTwistAngle();
 					bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
@@ -1257,7 +1266,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
 				tr = p6DOF->getCalculatedTransformB();
 				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				if(drawLimits) 
+				if(drawLimits)
 				{
 					tr = p6DOF->getCalculatedTransformA();
 					const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
@@ -1298,6 +1307,57 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 				}
 			}
 			break;
+		///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage
+		case D6_SPRING_2_CONSTRAINT_TYPE:
+		{
+			{
+				btGeneric6DofSpring2Constraint* p6DOF = (btGeneric6DofSpring2Constraint*)constraint;
+				btTransform tr = p6DOF->getCalculatedTransformA();
+				if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				tr = p6DOF->getCalculatedTransformB();
+				if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				if (drawLimits)
+				{
+					tr = p6DOF->getCalculatedTransformA();
+					const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
+					btVector3 up = tr.getBasis().getColumn(2);
+					btVector3 axis = tr.getBasis().getColumn(0);
+					btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
+					btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
+					btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
+					btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
+					getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
+					axis = tr.getBasis().getColumn(1);
+					btScalar ay = p6DOF->getAngle(1);
+					btScalar az = p6DOF->getAngle(2);
+					btScalar cy = btCos(ay);
+					btScalar sy = btSin(ay);
+					btScalar cz = btCos(az);
+					btScalar sz = btSin(az);
+					btVector3 ref;
+					ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
+					ref[1] = -sz*axis[0] + cz*axis[1];
+					ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
+					tr = p6DOF->getCalculatedTransformB();
+					btVector3 normal = -tr.getBasis().getColumn(0);
+					btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
+					btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
+					if (minFi > maxFi)
+					{
+						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false);
+					}
+					else if (minFi < maxFi)
+					{
+						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true);
+					}
+					tr = p6DOF->getCalculatedTransformA();
+					btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
+					btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
+					getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0));
+				}
+			}
+			break;
+		}
 		case SLIDER_CONSTRAINT_TYPE:
 			{
 				btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
@@ -1320,7 +1380,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 				}
 			}
 			break;
-		default : 
+		default :
 			break;
 	}
 	return;
@@ -1420,19 +1480,19 @@ void	btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serialize
 		worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm;
 		worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold;
 		worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp;
-		
+
 		worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop;
 		worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor;
 		worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce;
 		worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold;
-		
+
 		worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations;
 		worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode;
 		worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold;
 		worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize;
-		
+
 		worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
-	
+
 #ifdef BT_USE_DOUBLE_PRECISION
 		const char* structType = "btDynamicsWorldDoubleData";
 #else//BT_USE_DOUBLE_PRECISION
@@ -1448,10 +1508,10 @@ void	btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeDynamicsWorldInfo(serializer);
 
-	serializeRigidBodies(serializer);
-
 	serializeCollisionObjects(serializer);
 
+	serializeRigidBodies(serializer);
+
 	serializer->finishSerialization();
 }
 

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

@@ -151,7 +151,7 @@ public:
 	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
 
 
-	void	debugDrawConstraint(btTypedConstraint* constraint);
+	virtual void	debugDrawConstraint(btTypedConstraint* constraint);
 
 	virtual void	debugDrawWorld();
 

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

@@ -317,38 +317,50 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
 }
 
 
-bool btRigidBody::checkCollideWithOverride(const  btCollisionObject* co) const
-{
-	const btRigidBody* otherRb = btRigidBody::upcast(co);
-	if (!otherRb)
-		return true;
-
-	for (int i = 0; i < m_constraintRefs.size(); ++i)
-	{
-		const btTypedConstraint* c = m_constraintRefs[i];
-		if (c->isEnabled())
-			if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
-				return false;
-	}
-
-	return true;
-}
 
 
 
 void btRigidBody::addConstraintRef(btTypedConstraint* c)
 {
+	///disable collision with the 'other' body
+
 	int index = m_constraintRefs.findLinearSearch(c);
+	//don't add constraints that are already referenced
+	//btAssert(index == m_constraintRefs.size());
 	if (index == m_constraintRefs.size())
-		m_constraintRefs.push_back(c); 
-
-	m_checkCollideWith = true;
+	{
+		m_constraintRefs.push_back(c);
+		btCollisionObject* colObjA = &c->getRigidBodyA();
+		btCollisionObject* colObjB = &c->getRigidBodyB();
+		if (colObjA == this)
+		{
+			colObjA->setIgnoreCollisionCheck(colObjB, true);
+		}
+		else
+		{
+			colObjB->setIgnoreCollisionCheck(colObjA, true);
+		}
+	} 
 }
 
 void btRigidBody::removeConstraintRef(btTypedConstraint* c)
 {
-	m_constraintRefs.remove(c);
-	m_checkCollideWith = m_constraintRefs.size() > 0;
+	int index = m_constraintRefs.findLinearSearch(c);
+	//don't remove constraints that are not referenced
+	if(index < m_constraintRefs.size())
+    {
+        m_constraintRefs.remove(c);
+        btCollisionObject* colObjA = &c->getRigidBodyA();
+        btCollisionObject* colObjB = &c->getRigidBodyB();
+        if (colObjA == this)
+        {
+            colObjA->setIgnoreCollisionCheck(colObjB, false);
+        }
+        else
+        {
+            colObjB->setIgnoreCollisionCheck(colObjA, false);
+        }
+    }
 }
 
 int	btRigidBody::calculateSerializeBufferSize()	const

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

@@ -87,7 +87,7 @@ class btRigidBody  : public btCollisionObject
 	//m_optionalMotionState allows to automatic synchronize the world transform for active objects
 	btMotionState*	m_optionalMotionState;
 
-	//keep track of typed constraints referencing this rigid body
+	//keep track of typed constraints referencing this rigid body, to disable collision between linked bodies
 	btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
 
 	int				m_rigidbodyFlags;
@@ -506,8 +506,6 @@ public:
 		return (getBroadphaseProxy() != 0);
 	}
 
-	virtual bool checkCollideWithOverride(const  btCollisionObject* co) const;
-
 	void addConstraintRef(btTypedConstraint* c);
 	void removeConstraintRef(btTypedConstraint* c);
 

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

@@ -1,1009 +1,2896 @@
-/*
- * 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();
-    }
-}
+/*
+ * 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"
+#include "LinearMath/btTransformUtil.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
+}
+
+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);
+    }
+
+	void SpatialCrossProduct(const btVector3 &a_top,
+                            const btVector3 &a_bottom,
+                            const btVector3 &b_top,
+                            const btVector3 &b_bottom,
+							btVector3 &top_out,
+							btVector3 &bottom_out)
+	{
+		top_out = a_top.cross(b_top);
+		bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
+	}
+}
+
+
+//
+// Implementation of class btMultiBody
+//
+
+btMultiBody::btMultiBody(int n_links,
+                     btScalar mass,
+                     const btVector3 &inertia,
+                     bool fixedBase,
+                     bool canSleep,
+					 bool multiDof)
+    : 
+    	m_baseCollider(0),
+    	m_basePos(0,0,0),
+    	m_baseQuat(0, 0, 0, 1),
+      m_baseMass(mass),
+      m_baseInertia(inertia),
+    
+		m_fixedBase(fixedBase),
+		m_awake(true),
+		m_canSleep(canSleep),
+		m_sleepTimer(0),
+		
+		m_linearDamping(0.04f),
+		m_angularDamping(0.04f),
+		m_useGyroTerm(true),
+			m_maxAppliedImpulse(1000.f),
+		m_maxCoordinateVelocity(100.f),
+			m_hasSelfCollision(true),		
+		m_isMultiDof(multiDof),
+		__posUpdated(false),
+			m_dofCount(0),
+		m_posVarCnt(0),
+		m_useRK4(false), 	
+		m_useGlobalVelocities(false)
+{
+	
+	if(!m_isMultiDof)
+	{
+		m_vectorBuf.resize(2*n_links);	
+		m_realBuf.resize(6 + 2*n_links);
+		m_posVarCnt = n_links;
+	}
+   
+	m_links.resize(n_links);
+	m_matrixBuf.resize(n_links + 1);
+
+
+    m_baseForce.setValue(0, 0, 0);
+    m_baseTorque.setValue(0, 0, 0);
+}
+
+btMultiBody::~btMultiBody()
+{
+}
+
+void btMultiBody::setupFixed(int i,
+						   btScalar mass,
+						   const btVector3 &inertia,
+						   int parent,
+						   const btQuaternion &rotParentToThis,
+						   const btVector3 &parentComToThisPivotOffset,
+                           const btVector3 &thisPivotToThisComOffset,
+						   bool disableParentCollision)
+{
+	btAssert(m_isMultiDof);
+
+	m_links[i].m_mass = mass;
+    m_links[i].m_inertiaLocal = inertia;
+    m_links[i].m_parent = parent;
+    m_links[i].m_zeroRotParentToThis = rotParentToThis;
+	m_links[i].m_dVector = thisPivotToThisComOffset;
+    m_links[i].m_eVector = parentComToThisPivotOffset;    
+
+	m_links[i].m_jointType = btMultibodyLink::eFixed;
+	m_links[i].m_dofCount = 0;
+	m_links[i].m_posVarCount = 0;
+
+	if (disableParentCollision)
+		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;    
+	//
+	m_links[i].updateCacheMultiDof();
+
+	//
+	//if(m_isMultiDof)
+	//	resizeInternalMultiDofBuffers();
+	//
+	updateLinksDofOffsets();
+
+}
+
+
+void btMultiBody::setupPrismatic(int i,
+                               btScalar mass,
+                               const btVector3 &inertia,
+                               int parent,
+                               const btQuaternion &rotParentToThis,
+                               const btVector3 &jointAxis,
+                               const btVector3 &parentComToThisComOffset,
+							   const btVector3 &thisPivotToThisComOffset,
+							   bool disableParentCollision)
+{
+	if(m_isMultiDof)
+	{
+		m_dofCount += 1;
+		m_posVarCnt += 1;
+	}
+
+    m_links[i].m_mass = mass;
+    m_links[i].m_inertiaLocal = inertia;
+    m_links[i].m_parent = parent;
+    m_links[i].m_zeroRotParentToThis = rotParentToThis;
+    m_links[i].setAxisTop(0, 0., 0., 0.);
+    m_links[i].setAxisBottom(0, jointAxis);
+    m_links[i].m_eVector = parentComToThisComOffset;
+	m_links[i].m_dVector = thisPivotToThisComOffset;
+    m_links[i].m_cachedRotParentToThis = rotParentToThis;
+
+	m_links[i].m_jointType = btMultibodyLink::ePrismatic;
+	m_links[i].m_dofCount = 1;
+	m_links[i].m_posVarCount = 1;	
+	m_links[i].m_jointPos[0] = 0.f;
+	m_links[i].m_jointTorque[0] = 0.f;
+
+	if (disableParentCollision)
+		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+	//
+	if(m_isMultiDof)
+		m_links[i].updateCacheMultiDof();
+	else
+		m_links[i].updateCache();	
+	//
+	if(m_isMultiDof)
+		updateLinksDofOffsets();
+}
+
+void btMultiBody::setupRevolute(int i,
+                              btScalar mass,
+                              const btVector3 &inertia,
+                              int parent,
+                              const btQuaternion &rotParentToThis,
+                              const btVector3 &jointAxis,
+                              const btVector3 &parentComToThisPivotOffset,
+                              const btVector3 &thisPivotToThisComOffset,
+							  bool disableParentCollision)
+{
+	if(m_isMultiDof)
+	{
+		m_dofCount += 1;
+		m_posVarCnt += 1;
+	}
+
+    m_links[i].m_mass = mass;
+    m_links[i].m_inertiaLocal = inertia;
+    m_links[i].m_parent = parent;
+    m_links[i].m_zeroRotParentToThis = rotParentToThis;
+    m_links[i].setAxisTop(0, jointAxis);
+    m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
+    m_links[i].m_dVector = thisPivotToThisComOffset;
+    m_links[i].m_eVector = parentComToThisPivotOffset;
+
+	m_links[i].m_jointType = btMultibodyLink::eRevolute;
+	m_links[i].m_dofCount = 1;
+	m_links[i].m_posVarCount = 1;	
+	m_links[i].m_jointPos[0] = 0.f;
+	m_links[i].m_jointTorque[0] = 0.f;
+
+	if (disableParentCollision)
+		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+    //
+	if(m_isMultiDof)
+		m_links[i].updateCacheMultiDof();
+	else
+		m_links[i].updateCache();	
+	//
+	if(m_isMultiDof)
+		updateLinksDofOffsets();
+}
+
+
+
+void btMultiBody::setupSpherical(int i,
+						   btScalar mass,
+						   const btVector3 &inertia,
+						   int parent,
+						   const btQuaternion &rotParentToThis,
+						   const btVector3 &parentComToThisPivotOffset,
+						   const btVector3 &thisPivotToThisComOffset,
+						   bool disableParentCollision)
+{
+	btAssert(m_isMultiDof);
+	m_dofCount += 3;
+	m_posVarCnt += 4;
+
+	m_links[i].m_mass = mass;
+    m_links[i].m_inertiaLocal = inertia;
+    m_links[i].m_parent = parent;
+    m_links[i].m_zeroRotParentToThis = rotParentToThis;    
+    m_links[i].m_dVector = thisPivotToThisComOffset;
+    m_links[i].m_eVector = parentComToThisPivotOffset;    
+
+	m_links[i].m_jointType = btMultibodyLink::eSpherical;
+	m_links[i].m_dofCount = 3;
+	m_links[i].m_posVarCount = 4;
+	m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
+	m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
+	m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
+	m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
+	m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
+	m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
+	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointPos[3] = 1.f;
+	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
+
+
+	if (disableParentCollision)
+		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;    
+	//
+	m_links[i].updateCacheMultiDof();	
+	//
+	updateLinksDofOffsets();
+}
+
+#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+void btMultiBody::setupPlanar(int i,
+						   btScalar mass,
+						   const btVector3 &inertia,
+						   int parent,
+						   const btQuaternion &rotParentToThis,
+						   const btVector3 &rotationAxis,
+						   const btVector3 &parentComToThisComOffset,						   
+						   bool disableParentCollision)
+{
+	btAssert(m_isMultiDof);
+	m_dofCount += 3;
+	m_posVarCnt += 3;
+
+	m_links[i].m_mass = mass;
+    m_links[i].m_inertiaLocal = inertia;
+    m_links[i].m_parent = parent;
+    m_links[i].m_zeroRotParentToThis = rotParentToThis;    
+	m_links[i].m_dVector.setZero();
+    m_links[i].m_eVector = parentComToThisComOffset;
+
+	//
+	static btVector3 vecNonParallelToRotAxis(1, 0, 0);
+	if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
+		vecNonParallelToRotAxis.setValue(0, 1, 0);
+	//
+
+	m_links[i].m_jointType = btMultibodyLink::ePlanar;
+	m_links[i].m_dofCount = 3;
+	m_links[i].m_posVarCount = 3;
+	btVector3 n=rotationAxis.normalized();
+	m_links[i].setAxisTop(0, n[0],n[1],n[2]);
+	m_links[i].setAxisTop(1,0,0,0);
+	m_links[i].setAxisTop(2,0,0,0);
+	m_links[i].setAxisBottom(0,0,0,0);
+	btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
+	m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
+	cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
+	m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
+	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
+	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
+
+	if (disableParentCollision)
+		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+    //
+	m_links[i].updateCacheMultiDof();
+	//
+	updateLinksDofOffsets();
+}
+#endif
+
+void btMultiBody::finalizeMultiDof()
+{
+	btAssert(m_isMultiDof);
+
+	m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount);			//m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
+	m_vectorBuf.resize(2 * m_dofCount);													//two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
+
+	updateLinksDofOffsets();
+}
+	
+int btMultiBody::getParent(int i) const
+{
+    return m_links[i].m_parent;
+}
+
+btScalar btMultiBody::getLinkMass(int i) const
+{
+    return m_links[i].m_mass;
+}
+
+const btVector3 & btMultiBody::getLinkInertia(int i) const
+{
+    return m_links[i].m_inertiaLocal;
+}
+
+btScalar btMultiBody::getJointPos(int i) const
+{
+    return m_links[i].m_jointPos[0];
+}
+
+btScalar btMultiBody::getJointVel(int i) const
+{
+    return m_realBuf[6 + i];
+}
+
+btScalar * btMultiBody::getJointPosMultiDof(int i)
+{
+	return &m_links[i].m_jointPos[0];
+}
+
+btScalar * btMultiBody::getJointVelMultiDof(int i)
+{
+	return &m_realBuf[6 + m_links[i].m_dofOffset];
+}
+
+void btMultiBody::setJointPos(int i, btScalar q)
+{
+    m_links[i].m_jointPos[0] = q;
+    m_links[i].updateCache();
+}
+
+void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
+{
+	for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
+		m_links[i].m_jointPos[pos] = q[pos];
+
+    m_links[i].updateCacheMultiDof();
+}
+
+void btMultiBody::setJointVel(int i, btScalar qdot)
+{
+    m_realBuf[6 + i] = qdot;
+}
+
+void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot)
+{
+	for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof];
+}
+
+const btVector3 & btMultiBody::getRVector(int i) const
+{
+    return m_links[i].m_cachedRVector;
+}
+
+const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
+{
+    return m_links[i].m_cachedRotParentToThis;
+}
+
+btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
+{
+    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(m_baseQuat ,getBaseOmega());
+    vel[0] = quatRotate(m_baseQuat ,getBaseVel());
+    
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = m_links[i].m_parent;
+
+        // transform parent vel into this frame, store in omega[i+1], vel[i+1]
+        SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector,
+                         omega[parent+1], vel[parent+1],
+                         omega[i+1], vel[i+1]);
+
+        // now add qidot * shat_i
+        omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
+        vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
+    }
+}
+
+btScalar btMultiBody::getKineticEnergy() const
+{
+	int num_links = getNumLinks();
+    // TODO: would be better not to allocate memory here
+    btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
+	btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
+    compTreeLinkVelocities(&omega[0], &vel[0]);
+
+    // we will do the factor of 0.5 at the end
+    btScalar result = m_baseMass * vel[0].dot(vel[0]);
+    result += omega[0].dot(m_baseInertia * omega[0]);
+    
+    for (int i = 0; i < num_links; ++i) {
+        result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
+        result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]);
+    }
+
+    return 0.5f * result;
+}
+
+btVector3 btMultiBody::getAngularMomentum() const
+{
+	int num_links = getNumLinks();
+    // TODO: would be better not to allocate memory here
+    btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
+	btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
+    btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1);
+    compTreeLinkVelocities(&omega[0], &vel[0]);
+
+    rot_from_world[0] = m_baseQuat;
+    btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0]));
+
+    for (int i = 0; i < num_links; ++i) {
+        rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1];
+        result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
+    }
+
+    return result;
+}
+
+
+void btMultiBody::clearForcesAndTorques()
+{
+    m_baseForce.setValue(0, 0, 0);
+    m_baseTorque.setValue(0, 0, 0);
+
+	
+    for (int i = 0; i < getNumLinks(); ++i) {
+        m_links[i].m_appliedForce.setValue(0, 0, 0);
+        m_links[i].m_appliedTorque.setValue(0, 0, 0);
+		m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f;
+    }
+}
+
+void btMultiBody::clearVelocities()
+{
+	for (int i = 0; i < 6 + getNumLinks(); ++i) 
+	{
+		m_realBuf[i] = 0.f;
+	}
+}
+void btMultiBody::addLinkForce(int i, const btVector3 &f)
+{
+    m_links[i].m_appliedForce += f;
+}
+
+void btMultiBody::addLinkTorque(int i, const btVector3 &t)
+{
+    m_links[i].m_appliedTorque += t;
+}
+
+void btMultiBody::addJointTorque(int i, btScalar Q)
+{
+    m_links[i].m_jointTorque[0] += Q;
+}
+
+void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q)
+{
+	m_links[i].m_jointTorque[dof] += Q;
+}
+
+void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q)
+{
+	for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		m_links[i].m_jointTorque[dof] = Q[dof];
+}
+
+const btVector3 & btMultiBody::getLinkForce(int i) const
+{
+    return m_links[i].m_appliedForce;
+}
+
+const btVector3 & btMultiBody::getLinkTorque(int i) const
+{
+    return m_links[i].m_appliedTorque;
+}
+
+btScalar btMultiBody::getJointTorque(int i) const
+{
+    return m_links[i].m_jointTorque[0];
+}
+
+btScalar * btMultiBody::getJointTorqueMultiDof(int i)
+{
+    return &m_links[i].m_jointTorque[0];
+}
+
+inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1)				//renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?
+{
+		btVector3 row0 = btVector3( 
+			v0.x() * v1.x(),
+			v0.x() * v1.y(),
+			v0.x() * v1.z());
+		btVector3 row1 = btVector3( 
+			v0.y() * v1.x(),
+			v0.y() * v1.y(),
+			v0.y() * v1.z());
+		btVector3 row2 = btVector3( 
+			v0.z() * v1.x(),
+			v0.z() * v1.y(),
+			v0.z() * v1.z());
+
+        btMatrix3x3 m(row0[0],row0[1],row0[2],
+						row1[0],row1[1],row1[2],
+						row2[0],row2[1],row2[2]);
+		return m;
+}
+
+#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
+//
+
+#ifndef TEST_SPATIAL_ALGEBRA_LAYER
+void btMultiBody::stepVelocitiesMultiDof(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*m_dofCount + 6);				//multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
+    scratch_v.resize(8*num_links + 6);
+    scratch_m.resize(4*num_links + 4);
+
+    btScalar * r_ptr = &scratch_r[0];
+    btScalar * output = &scratch_r[m_dofCount];  // "output" holds the q_double_dot results
+    btVector3 * v_ptr = &scratch_v[0];
+    
+    // vhat_i  (top = angular, bottom = linear part)
+    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 * zeroAccForce = v_ptr; v_ptr += num_links + 1;
+    btVector3 * zeroAccTorque = 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 = &m_matrixBuf[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 = m_dofCount > 0 ? &m_vectorBuf[0] : 0;
+    btVector3 * h_bottom = m_dofCount > 0 ? &m_vectorBuf[m_dofCount] : 0;
+    btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
+    btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
+
+    // Y_i, invD_i
+    btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
+	btScalar * Y = &scratch_r[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(m_baseQuat);				//m_baseQuat assumed to be alias!?
+
+    vel_top_angular[0] = rot_from_parent[0] * base_omega;
+    vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
+
+    if (m_fixedBase) 
+	{
+        zeroAccForce[0] = zeroAccTorque[0] = btVector3(0,0,0);
+    }
+	else 
+	{
+        zeroAccForce[0] = - (rot_from_parent[0] * (m_baseForce 
+                                                   - m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
+		
+        zeroAccTorque[0] =
+            - (rot_from_parent[0] * m_baseTorque);
+
+		if (m_useGyroTerm)
+			zeroAccTorque[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
+
+        zeroAccTorque[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
+
+		//NOTE: Coriolis terms are missing! (left like so following Stephen's code)
+    }
+
+    inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);
+	
+	
+    inertia_top_right[0].setValue(m_baseMass, 0, 0,
+                            0, m_baseMass, 0,
+                            0, 0, m_baseMass);
+    inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0,
+                              0, m_baseInertia[1], 0,
+                              0, 0, m_baseInertia[2]);
+
+    rot_from_world[0] = rot_from_parent[0];
+
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = m_links[i].m_parent;
+        rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
+		
+
+        rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
+        
+        // vhat_i = i_xhat_p(i) * vhat_p(i)
+        SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
+                         vel_top_angular[parent+1], vel_bottom_linear[parent+1],
+                         vel_top_angular[i+1], vel_bottom_linear[i+1]);
+		//////////////////////////////////////////////////////////////		
+        
+        // now set vhat_i to its true value by doing
+        // vhat_i += qidot * shat_i        
+		btVector3 joint_vel_spat_top, joint_vel_spat_bottom;
+		joint_vel_spat_top.setZero(); joint_vel_spat_bottom.setZero();
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			joint_vel_spat_top += getJointVelMultiDof(i)[dof] * m_links[i].getAxisTop(dof);
+			joint_vel_spat_bottom += getJointVelMultiDof(i)[dof] * m_links[i].getAxisBottom(dof);
+		}
+
+		vel_top_angular[i+1] += joint_vel_spat_top;
+		vel_bottom_linear[i+1] += joint_vel_spat_bottom;
+
+		// we can now calculate chat_i
+        // remember vhat_i is really vhat_p(i) (but in current frame) at this point
+		SpatialCrossProduct(vel_top_angular[i+1], vel_bottom_linear[i+1], joint_vel_spat_top, joint_vel_spat_bottom, coriolis_top_angular[i], coriolis_bottom_linear[i]);
+
+        // calculate zhat_i^A
+		//
+		//external forces
+        zeroAccForce[i+1] = -(rot_from_world[i+1] * (m_links[i].m_appliedForce));
+		zeroAccTorque[i+1] = -(rot_from_world[i+1] * m_links[i].m_appliedTorque);
+		//
+		//DAMPING TERMS (ONLY)
+        zeroAccForce[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
+        zeroAccTorque[i+1] += m_links[i].m_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(m_links[i].m_mass, 0, 0,
+										0, m_links[i].m_mass, 0,
+										0, 0, m_links[i].m_mass);
+        inertia_bottom_left[i+1].setValue(m_links[i].m_inertia[0], 0, 0,
+										0, m_links[i].m_inertia[1], 0,
+										0, 0, m_links[i].m_inertia[2]);
+
+
+		////
+		//p += v x* Iv - done in a simpler way		
+		if(m_useGyroTerm)
+			zeroAccTorque[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertia * vel_top_angular[i+1] );
+		//
+		coriolis_bottom_linear[i] += vel_top_angular[i+1].cross(vel_bottom_linear[i+1]) - (rot_from_parent[i+1] * (vel_top_angular[parent+1].cross(vel_bottom_linear[parent+1])));
+		//coriolis_bottom_linear[i].setZero();
+
+		//printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
+		//printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());		
+		//printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
+    }
+
+	static btScalar D[36];				//it's dofxdof for each body so asingle 6x6 D matrix will do
+    // 'Downward' loop.
+    // (part of TreeForwardDynamics in Mirtich.)
+    for (int i = num_links - 1; i >= 0; --i)
+	{		
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
+			btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
+
+			//pFunMultSpatVecTimesSpatMat2(m_links[i].m_axesTop[dof], m_links[i].m_axesBottom[dof], inertia_top_left[i+1], inertia_top_right[i+1], inertia_bottom_left[i+1], h_t, h_b);
+			{
+				h_t = inertia_top_left[i+1] * m_links[i].getAxisTop(dof) + inertia_top_right[i+1] * m_links[i].getAxisBottom(dof);
+				h_b = inertia_bottom_left[i+1] * m_links[i].getAxisTop(dof) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(dof);
+			}			
+
+			btScalar *D_row = &D[dof * m_links[i].m_dofCount];
+			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			{
+				D_row[dof2] = SpatialDotProduct(m_links[i].getAxisTop(dof2), m_links[i].getAxisBottom(dof2), h_t, h_b);
+			}
+
+			Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
+											- SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1])
+											- SpatialDotProduct(h_t, h_b, coriolis_top_angular[i], coriolis_bottom_linear[i])
+											;
+		}
+
+        const int parent = m_links[i].m_parent;
+
+        btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
+		switch(m_links[i].m_jointType)
+		{
+			case btMultibodyLink::ePrismatic:
+			case btMultibodyLink::eRevolute:
+			{
+				invDi[0] = 1.0f / D[0];
+				break;
+			}
+			case btMultibodyLink::eSpherical:
+#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+			case btMultibodyLink::ePlanar:
+#endif
+			{
+				static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
+				static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
+
+				//unroll the loop?
+				for(int row = 0; row < 3; ++row)
+					for(int col = 0; col < 3; ++col)
+						invDi[row * 3 + col] = invD3x3[row][col];
+
+				break;
+			}
+			default:
+			{
+			}
+		}
+		
+
+		static btVector3 tmp_top[6];															//move to scratch mem or other buffers? (12 btVector3 will suffice)
+		static btVector3 tmp_bottom[6];
+
+		//for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		//{
+		//	tmp_top[dof].setZero();
+		//	tmp_bottom[dof].setZero();
+
+		//	for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+		//	{
+		//		btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof2];
+		//		btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof2];
+		//		//
+		//		tmp_top[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * h_b;
+		//		tmp_bottom[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * h_t;
+		//	}
+		//}
+
+		//btMatrix3x3 TL = inertia_top_left[i+1], TR = inertia_top_right[i+1], BL = inertia_bottom_left[i+1];
+
+		//for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		//{
+		//	btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
+		//	btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
+
+		//	TL -= outerProduct(h_t, tmp_top[dof]);
+		//	TR -= outerProduct(h_t, tmp_bottom[dof]);
+		//	BL -= outerProduct(h_b, tmp_top[dof]);
+		//}
+
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			tmp_top[dof].setZero();
+			tmp_bottom[dof].setZero();			
+
+			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			{		
+				btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof2];
+				btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof2];
+				//
+				tmp_top[dof] += invDi[dof2 * m_links[i].m_dofCount + dof] * h_t;
+				tmp_bottom[dof] += invDi[dof2 * m_links[i].m_dofCount + dof] * h_b;
+			}
+		}
+
+		btMatrix3x3 TL = inertia_top_left[i+1], TR = inertia_top_right[i+1], BL = inertia_bottom_left[i+1];
+
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
+			btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
+
+			TL -= outerProduct(h_t, tmp_bottom[dof]);
+			TR -= outerProduct(h_t, tmp_top[dof]);
+			BL -= outerProduct(h_b, tmp_bottom[dof]);
+		}
+
+
+        btMatrix3x3 r_cross;
+        r_cross.setValue(
+            0, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1],
+            m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0],
+            -m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[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];
+        
+        
+        btVector3 in_top, in_bottom, out_top, out_bottom;        
+
+		static btScalar invD_times_Y[6];													//D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; definitely move to buffers; num_dof of btScalar would cover all bodies but acutally 6 btScalars will also be okay
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			invD_times_Y[dof] = 0.f;
+
+			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			{
+				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];				
+			}	
+		}
+
+		in_top = zeroAccForce[i+1]
+            + inertia_top_left[i+1] * coriolis_top_angular[i]
+            + inertia_top_right[i+1] * coriolis_bottom_linear[i];
+            
+        in_bottom = zeroAccTorque[i+1]
+            + inertia_bottom_left[i+1] * coriolis_top_angular[i]
+            + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i];
+
+		//unroll the loop?
+		for(int row = 0; row < 3; ++row)
+		{
+			for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+			{
+				btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
+				btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
+
+				in_top[row] += h_t[row] * invD_times_Y[dof];
+				in_bottom[row] += h_b[row] * invD_times_Y[dof];			
+			}
+		}			
+			
+		InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
+                                in_top, in_bottom, out_top, out_bottom);		
+
+        zeroAccForce[parent+1] += out_top;
+        zeroAccTorque[parent+1] += out_bottom;
+    }
+
+
+    // Second 'upward' loop
+    // (part of TreeForwardDynamics in Mirtich)
+
+    if (m_fixedBase) 
+	{
+        accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
+    } 
+	else 
+	{
+        if (num_links > 0) 
+		{
+			m_cachedInertiaTopLeft = inertia_top_left[0];
+			m_cachedInertiaTopRight = inertia_top_right[0];
+			m_cachedInertiaLowerLeft = inertia_bottom_left[0];
+			m_cachedInertiaLowerRight= inertia_top_left[0].transpose();
+
+        }
+		btVector3 rhs_top (zeroAccForce[0][0], zeroAccForce[0][1], zeroAccForce[0][2]);
+		btVector3 rhs_bot (zeroAccTorque[0][0], zeroAccTorque[0][1], zeroAccTorque[0][2]);
+        float result[6];
+
+		solveImatrix(rhs_top, rhs_bot, result);
+        for (int i = 0; i < 3; ++i) {
+            accel_top[0][i] = -result[i];
+            accel_bottom[0][i] = -result[i+3];
+        }
+
+    }
+
+	static btScalar Y_minus_hT_a[6];					//it's dofx1 for each body so a single 6x1 temp is enough
+    // now do the loop over the m_links
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = m_links[i].m_parent;
+
+		SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
+                         accel_top[parent+1], accel_bottom[parent+1],
+                         accel_top[i+1], accel_bottom[i+1]);		
+		
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
+			btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
+
+			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - SpatialDotProduct(h_t, h_b, accel_top[i+1], accel_bottom[i+1]);
+		}
+
+		btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
+		mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
+
+		accel_top[i+1] += coriolis_top_angular[i];
+		accel_bottom[i+1] += coriolis_bottom_linear[i];
+
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			accel_top[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisTop(dof);
+			accel_bottom[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisBottom(dof);
+		}        
+    }
+
+    // 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];
+
+	/////////////////
+	//printf("q = [");
+	//printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
+	//for(int link = 0; link < getNumLinks(); ++link)
+	//	for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
+	//		printf("%.6f ", m_links[link].m_jointPos[dof]);
+	//printf("]\n");
+	////
+	//printf("qd = [");
+	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
+	//	printf("%.6f ", m_realBuf[dof]);
+	//printf("]\n");
+	//printf("qdd = [");
+	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
+	//	printf("%.6f ", output[dof]);
+	//printf("]\n");
+	/////////////////
+
+    // Final step: add the accelerations (times dt) to the velocities.
+    applyDeltaVeeMultiDof(output, dt);	
+}
+
+#else					//i.e. TEST_SPATIAL_ALGEBRA_LAYER
+void btMultiBody::stepVelocitiesMultiDof(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*m_dofCount + 6);				//multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
+    scratch_v.resize(8*num_links + 6);
+    scratch_m.resize(4*num_links + 4);
+
+	//btScalar * r_ptr = &scratch_r[0];
+    btScalar * output = &scratch_r[m_dofCount];  // "output" holds the q_double_dot results
+    btVector3 * v_ptr = &scratch_v[0];
+    
+    // vhat_i  (top = angular, bottom = linear part)	
+	btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;
+	v_ptr += num_links * 2 + 2;
+	//
+    // zhat_i^A    
+	btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
+	v_ptr += num_links * 2 + 2;
+	//
+    // chat_i  (note NOT defined for the base)    
+	btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
+	v_ptr += num_links * 2;
+	//
+    // Ihat_i^A.    
+	btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];
+
+    // Cached 3x3 rotation matrices from parent frame to this frame.
+    btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
+    btMatrix3x3 * rot_from_world = &scratch_m[0];
+
+    // hhat_i, ahat_i
+    // hhat is NOT stored for the base (but ahat is)    
+	btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
+	btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
+	v_ptr += num_links * 2 + 2;
+	//
+    // Y_i, invD_i
+    btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
+	btScalar * Y = &scratch_r[0];
+	//
+	//aux variables	
+	static btSpatialMotionVector spatJointVel;					//spatial velocity due to the joint motion (i.e. without predecessors' influence)
+	static btScalar D[36];										//"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do	
+	static btScalar invD_times_Y[6];							//D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies	
+	static btSpatialMotionVector result;							//holds results of the SolveImatrix op; it is a spatial motion vector (accel)
+	static btScalar Y_minus_hT_a[6];							//Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough	
+	static btSpatialForceVector spatForceVecTemps[6];				//6 temporary spatial force vectors
+	static btSpatialTransformationMatrix fromParent;				//spatial transform from parent to child
+	static btSymmetricSpatialDyad dyadTemp;						//inertia matrix temp
+	static btSpatialTransformationMatrix fromWorld;
+	fromWorld.m_trnVec.setZero();
+	/////////////////
+
+    // ptr to the joint accel part of the output
+    btScalar * joint_accel = output + 6;
+
+    // Start of the algorithm proper.
+    
+    // First 'upward' loop.
+    // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
+
+    rot_from_parent[0] = btMatrix3x3(m_baseQuat);				//m_baseQuat assumed to be alias!?
+
+	//create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
+	spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
+
+    if (m_fixedBase) 
+	{        
+		zeroAccSpatFrc[0].setZero();
+    }
+	else 
+	{
+		//external forces		
+		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * m_baseTorque), -(rot_from_parent[0] * m_baseForce));	
+
+		//adding damping terms (only)
+		btScalar linDampMult = 1., angDampMult = 1.;
+		zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()),
+								   linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm()));
+
+		//
+		//p += vhat x Ihat vhat - done in a simpler way
+		if (m_useGyroTerm)
+			zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));
+		//
+		zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
+    }
+
+
+	//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
+	spatInertia[0].setMatrix(	btMatrix3x3(0,0,0,0,0,0,0,0,0),
+								//
+								btMatrix3x3(m_baseMass, 0, 0,
+											0, m_baseMass, 0,
+											0, 0, m_baseMass),
+								//
+								btMatrix3x3(m_baseInertia[0], 0, 0,
+											0, m_baseInertia[1], 0,
+											0, 0, m_baseInertia[2])
+							);
+
+    rot_from_world[0] = rot_from_parent[0];
+
+	//
+    for (int i = 0; i < num_links; ++i) {		
+        const int parent = m_links[i].m_parent;
+        rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
+        rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
+
+		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
+		fromWorld.m_rotMat = rot_from_world[i+1];
+		fromParent.transform(spatVel[parent+1], spatVel[i+1]);
+
+		// now set vhat_i to its true value by doing
+        // vhat_i += qidot * shat_i			
+		if(!m_useGlobalVelocities)
+		{
+			spatJointVel.setZero();
+
+			for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
+				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
+
+			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
+			spatVel[i+1] += spatJointVel;
+
+			//
+			// vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
+			//spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
+
+		}
+		else
+		{
+			fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]);
+			fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
+		}
+
+		// we can now calculate chat_i 		
+		spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]);		
+
+        // calculate zhat_i^A
+		//
+		//external forces		
+		zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * m_links[i].m_appliedTorque), -(rot_from_world[i+1] * m_links[i].m_appliedForce));
+		//
+		//adding damping terms (only)
+		btScalar linDampMult = 1., angDampMult = 1.;
+		zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
+									 linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
+		
+        // calculate Ihat_i^A
+		//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
+		spatInertia[i+1].setMatrix(	btMatrix3x3(0,0,0,0,0,0,0,0,0),
+									//
+									btMatrix3x3(m_links[i].m_mass, 0, 0,
+												0, m_links[i].m_mass, 0,
+												0, 0, m_links[i].m_mass),
+									//
+									btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
+												0, m_links[i].m_inertiaLocal[1], 0,
+												0, 0, m_links[i].m_inertiaLocal[2])
+								);
+		//
+		//p += vhat x Ihat vhat - done in a simpler way
+		if(m_useGyroTerm)
+			zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));			
+		//		
+		zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
+		//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
+		////clamp parent's omega
+		//btScalar parOmegaMod = temp.length();
+		//btScalar parOmegaModMax = 1000;
+		//if(parOmegaMod > parOmegaModMax)
+		//	temp *= parOmegaModMax / parOmegaMod;
+		//zeroAccSpatFrc[i+1].addLinear(temp);
+		//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
+		//temp = spatCoriolisAcc[i].getLinear();
+		//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
+		
+		
+
+		//printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
+		//printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());		
+		//printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
+    }
+	
+    // 'Downward' loop.
+    // (part of TreeForwardDynamics in Mirtich.)
+    for (int i = num_links - 1; i >= 0; --i)
+	{
+		const int parent = m_links[i].m_parent;
+		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
+
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			//
+			hDof = spatInertia[i+1] * m_links[i].m_axes[dof];
+			//
+			Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
+			- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
+			- spatCoriolisAcc[i].dot(hDof)
+			;
+		}
+
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			btScalar *D_row = &D[dof * m_links[i].m_dofCount];			
+			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			{
+				btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
+				D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
+			}
+		}
+
+        btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
+		switch(m_links[i].m_jointType)
+		{
+			case btMultibodyLink::ePrismatic:
+			case btMultibodyLink::eRevolute:
+			{
+				invDi[0] = 1.0f / D[0];
+				break;
+			}
+			case btMultibodyLink::eSpherical:
+#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+			case btMultibodyLink::ePlanar:
+#endif
+			{
+				static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
+				static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
+
+				//unroll the loop?
+				for(int row = 0; row < 3; ++row)
+				{
+					for(int col = 0; col < 3; ++col)
+					{						
+						invDi[row * 3 + col] = invD3x3[row][col];
+					}
+				}
+
+				break;
+			}
+			default:
+			{
+			
+			}
+		}
+
+		//determine h*D^{-1}
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			spatForceVecTemps[dof].setZero();
+
+			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			{				
+				btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
+				//				
+				spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
+			}
+		}
+
+		dyadTemp = spatInertia[i+1];
+
+		//determine (h*D^{-1}) * h^{T}
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{			
+			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			//
+			dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
+		}
+
+		fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add);
+        
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			invD_times_Y[dof] = 0.f;
+
+			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			{
+				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];				
+			}	
+		}
+		
+		spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i];		
+
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{				
+			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			//
+			spatForceVecTemps[0] += hDof * invD_times_Y[dof];			
+		}
+		
+		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
+		
+		zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
+    }
+
+
+    // Second 'upward' loop
+    // (part of TreeForwardDynamics in Mirtich)
+
+    if (m_fixedBase) 
+	{
+        spatAcc[0].setZero();
+    } 
+	else 
+	{
+        if (num_links > 0) 
+		{
+			m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
+			m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
+			m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
+			m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose();
+
+        }		
+		
+		solveImatrix(zeroAccSpatFrc[0], result);
+		spatAcc[0] = -result;
+    }
+	
+    // now do the loop over the m_links
+    for (int i = 0; i < num_links; ++i) 
+	{
+		//	qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)
+		//	a = apar + cor + Sqdd
+		//or
+		//	qdd = D^{-1} * (Y - h^{T}*(apar+cor))
+		//	a = apar + Sqdd
+
+        const int parent = m_links[i].m_parent;
+		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
+
+		fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
+		
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			//			
+			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);			
+		}
+
+		btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
+		//D^{-1} * (Y - h^{T}*apar)
+		mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
+
+		spatAcc[i+1] += spatCoriolisAcc[i];		
+
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
+			spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
+    }
+
+    // transform base accelerations back to the world frame.
+    btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
+	output[0] = omegadot_out[0];
+	output[1] = omegadot_out[1];
+	output[2] = omegadot_out[2];
+
+    btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
+	output[3] = vdot_out[0];
+	output[4] = vdot_out[1];
+	output[5] = vdot_out[2];
+
+	/////////////////
+	//printf("q = [");
+	//printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
+	//for(int link = 0; link < getNumLinks(); ++link)
+	//	for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
+	//		printf("%.6f ", m_links[link].m_jointPos[dof]);
+	//printf("]\n");
+	////
+	//printf("qd = [");
+	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
+	//	printf("%.6f ", m_realBuf[dof]);
+	//printf("]\n");
+	//printf("qdd = [");
+	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
+	//	printf("%.6f ", output[dof]);
+	//printf("]\n");
+	/////////////////
+
+    // Final step: add the accelerations (times dt) to the velocities.
+	if(dt > 0.)
+		applyDeltaVeeMultiDof(output, dt);
+
+	/////
+	//btScalar angularThres = 1;
+	//btScalar maxAngVel = 0.;		
+	//bool scaleDown = 1.;
+	//for(int link = 0; link < m_links.size(); ++link)
+	//{		
+	//	if(spatVel[link+1].getAngular().length() > maxAngVel)
+	//	{
+	//		maxAngVel = spatVel[link+1].getAngular().length();
+	//		scaleDown = angularThres / spatVel[link+1].getAngular().length();
+	//		break;
+	//	}		
+	//}
+
+	//if(scaleDown != 1.)
+	//{
+	//	for(int link = 0; link < m_links.size(); ++link)
+	//	{
+	//		if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)
+	//		{
+	//			for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
+	//				getJointVelMultiDof(link)[dof] *= scaleDown;
+	//		}
+	//	}
+	//}
+	/////
+
+	/////////////////////
+	if(m_useGlobalVelocities)
+	{
+		for (int i = 0; i < num_links; ++i) 
+		{
+			const int parent = m_links[i].m_parent;
+			//rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);    /// <- done
+			//rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];		/// <- done
+		
+			fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
+			fromWorld.m_rotMat = rot_from_world[i+1];			
+        
+			// vhat_i = i_xhat_p(i) * vhat_p(i)		
+			fromParent.transform(spatVel[parent+1], spatVel[i+1]);
+			//nice alternative below (using operator *) but it generates temps
+			/////////////////////////////////////////////////////////////
+
+			// now set vhat_i to its true value by doing
+			// vhat_i += qidot * shat_i			
+			spatJointVel.setZero();
+
+			for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
+				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
+		
+			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
+			spatVel[i+1] += spatJointVel;
+
+
+			fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity);
+			fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
+		}
+	}
+	
+}
+#endif
+
+
+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 = &m_matrixBuf[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 ? &m_vectorBuf[0] : 0;
+    btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[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_realBuf[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(m_baseQuat);
+
+    vel_top_angular[0] = rot_from_parent[0] * base_omega;
+    vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
+
+    if (m_fixedBase) {
+        zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
+    } else {
+        zero_acc_top_angular[0] = - (rot_from_parent[0] * (m_baseForce 
+                                                   - m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
+		
+        zero_acc_bottom_linear[0] =
+            - (rot_from_parent[0] * m_baseTorque);
+
+		if (m_useGyroTerm)
+			zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
+
+        zero_acc_bottom_linear[0] += m_baseInertia * 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(m_baseMass, 0, 0,
+                            0, m_baseMass, 0,
+                            0, 0, m_baseMass);
+    inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0,
+                              0, m_baseInertia[1], 0,
+                              0, 0, m_baseInertia[2]);
+
+    rot_from_world[0] = rot_from_parent[0];
+
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = m_links[i].m_parent;
+        rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
+		
+
+        rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
+        
+        // vhat_i = i_xhat_p(i) * vhat_p(i)
+        SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
+                         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(m_links[i].m_cachedRVector))
+            + 2 * vel_top_angular[i+1].cross(m_links[i].getAxisBottom(0)) * getJointVel(i);
+		if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
+            coriolis_top_angular[i] = vel_top_angular[i+1].cross(m_links[i].getAxisTop(0)) * getJointVel(i);
+            coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * m_links[i].getAxisTop(0).cross(m_links[i].getAxisBottom(0));
+        } 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) * m_links[i].getAxisTop(0);
+        vel_bottom_linear[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
+
+        // calculate zhat_i^A
+        zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (m_links[i].m_appliedForce));
+        zero_acc_top_angular[i+1] += m_links[i].m_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] * m_links[i].m_appliedTorque);
+		if (m_useGyroTerm)
+		{
+			zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
+		}
+
+        zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * 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(m_links[i].m_mass, 0, 0,
+                                  0, m_links[i].m_mass, 0,
+                                  0, 0, m_links[i].m_mass);
+        inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0,
+                                    0, m_links[i].m_inertiaLocal[1], 0,
+                                    0, 0, m_links[i].m_inertiaLocal[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] * m_links[i].getAxisTop(0) + inertia_top_right[i+1] * m_links[i].getAxisBottom(0);
+        h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0);
+		btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]);
+        D[i] = val;
+		
+		Y[i] = m_links[i].m_jointTorque[0]
+            - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), 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 = m_links[i].m_parent;
+
+        btAssert(D[i]!=0.f);
+        // 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, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1],
+            m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0],
+            -m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[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], m_links[i].m_cachedRVector,
+                                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 (m_fixedBase) 
+	{
+        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?
+
+			m_cachedInertiaTopLeft = inertia_top_left[0];
+			m_cachedInertiaTopRight = inertia_top_right[0];
+			m_cachedInertiaLowerLeft = inertia_bottom_left[0];
+			m_cachedInertiaLowerRight= 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 m_links
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = m_links[i].m_parent;
+        SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
+                         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] * m_links[i].getAxisTop(0);
+        accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * m_links[i].getAxisBottom(0);
+    }
+
+    // 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];
+
+	/////////////////
+	//printf("q = [");
+	//printf("%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
+	//for(int link = 0; link < getNumLinks(); ++link)
+	//	printf("%.2f ", m_links[link].m_jointPos[0]);
+	//printf("]\n");
+	////
+	//printf("qd = [");
+	//for(int dof = 0; dof < getNumLinks() + 6; ++dof)
+	//	printf("%.2f ", m_realBuf[dof]);
+	//printf("]\n");
+	////
+	//printf("qdd = [");
+	//for(int dof = 0; dof < getNumLinks() + 6; ++dof)
+	//	printf("%.2f ", output[dof]);
+	//printf("]\n");
+	/////////////////
+
+    // 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 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
+        result[0] = rhs_bot[0] / m_baseInertia[0];
+        result[1] = rhs_bot[1] / m_baseInertia[1];
+        result[2] = rhs_bot[2] / m_baseInertia[2];
+        result[3] = rhs_top[0] / m_baseMass;
+        result[4] = rhs_top[1] / m_baseMass;
+        result[5] = rhs_top[2] / m_baseMass;
+    } else 
+	{
+		/// Special routine for calculating the inverse of a spatial inertia matrix
+		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
+		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
+		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
+		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
+		tmp = invIupper_right * m_cachedInertiaLowerRight;
+		btMatrix3x3 invI_upper_left = (tmp * Binv);
+		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
+		tmp = m_cachedInertiaTopLeft  * invI_upper_left;
+		tmp[0][0]-= 1.0;
+		tmp[1][1]-= 1.0;
+		tmp[2][2]-= 1.0;
+		btMatrix3x3 invI_lower_left = (Binv * tmp);
+
+		//multiply result = invI * rhs
+		{
+		  btVector3 vtop = invI_upper_left*rhs_top;
+		  btVector3 tmp;
+		  tmp = invIupper_right * rhs_bot;
+		  vtop += tmp;
+		  btVector3 vbot = invI_lower_left*rhs_top;
+		  tmp = invI_lower_right * rhs_bot;
+		  vbot += tmp;
+		  result[0] = vtop[0];
+		  result[1] = vtop[1];
+		  result[2] = vtop[2];
+		  result[3] = vbot[0];
+		  result[4] = vbot[1];
+		  result[5] = vbot[2];
+		}
+		
+    }
+}
+#ifdef TEST_SPATIAL_ALGEBRA_LAYER
+void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
+{
+	int num_links = getNumLinks();
+	///solve I * x = rhs, so the result = invI * rhs
+    if (num_links == 0) 
+	{
+		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
+		result.setAngular(rhs.getAngular() / m_baseInertia);
+		result.setLinear(rhs.getLinear() / m_baseMass);		
+    } else 
+	{
+		/// Special routine for calculating the inverse of a spatial inertia matrix
+		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
+		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
+		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
+		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
+		tmp = invIupper_right * m_cachedInertiaLowerRight;
+		btMatrix3x3 invI_upper_left = (tmp * Binv);
+		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
+		tmp = m_cachedInertiaTopLeft  * invI_upper_left;
+		tmp[0][0]-= 1.0;
+		tmp[1][1]-= 1.0;
+		tmp[2][2]-= 1.0;
+		btMatrix3x3 invI_lower_left = (Binv * tmp);
+
+		//multiply result = invI * rhs
+		{
+		  btVector3 vtop = invI_upper_left*rhs.getLinear();
+		  btVector3 tmp;
+		  tmp = invIupper_right * rhs.getAngular();
+		  vtop += tmp;
+		  btVector3 vbot = invI_lower_left*rhs.getLinear();
+		  tmp = invI_lower_right * rhs.getAngular();
+		  vbot += tmp;
+		  result.setVector(vtop, vbot);		  
+		}
+		
+    }
+}
+#endif
+
+void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
+{
+	for (int row = 0; row < rowsA; row++)
+	{
+		for (int col = 0; col < colsB; col++)
+		{
+			pC[row * colsB + col] = 0.f;
+			for (int inner = 0; inner < rowsB; inner++)
+			{
+				pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
+			}
+		}
+	}
+}
+
+#ifndef TEST_SPATIAL_ALGEBRA_LAYER
+void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
+                                       btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
+{
+    // Temporary matrices/vectors -- use scratch space from caller
+    // so that we don't have to keep reallocating every frame
+
+	int num_links = getNumLinks();
+	int m_dofCount = getNumDofs();
+    scratch_r.resize(m_dofCount);
+    scratch_v.resize(4*num_links + 4);	    
+
+    btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0;
+    btVector3 * v_ptr = &scratch_v[0];
+
+    // zhat_i^A (scratch space)
+    btVector3 * zeroAccForce = v_ptr; v_ptr += num_links + 1;
+    btVector3 * zeroAccTorque = v_ptr; v_ptr += num_links + 1;
+
+    // rot_from_parent (cached from calcAccelerations)
+    const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
+
+    // hhat (cached), accel (scratch)
+    // hhat is NOT stored for the base (but ahat is) 
+	const btVector3 * h_top = m_dofCount > 0 ? &m_vectorBuf[0] : 0;
+    const btVector3 * h_bottom = m_dofCount > 0 ? &m_vectorBuf[m_dofCount] : 0;	
+    btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
+    btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
+
+    // Y_i (scratch), invD_i (cached)
+    const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
+	btScalar * Y = r_ptr;
+	////////////////
+
+    // 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 (m_fixedBase) 
+	{
+        zeroAccForce[0] = zeroAccTorque[0] = btVector3(0,0,0);
+    } else 
+	{
+        zeroAccForce[0] = - (rot_from_parent[0] * input_force);
+        zeroAccTorque[0] =  - (rot_from_parent[0] * input_torque);
+    }
+    for (int i = 0; i < num_links; ++i) 
+	{
+        zeroAccForce[i+1] = zeroAccTorque[i+1] = btVector3(0,0,0);
+    }    
+
+	// 'Downward' loop.
+    // (part of TreeForwardDynamics in Mirtich.)
+    for (int i = num_links - 1; i >= 0; --i)
+	{		
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+//??			btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]);
+
+			Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
+											- SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1])											
+											;
+		}
+
+		btScalar aa = Y[i];
+        const int parent = m_links[i].m_parent;
+
+		btVector3 in_top, in_bottom, out_top, out_bottom;
+		const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
+
+		static btScalar invD_times_Y[6];													//D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; definitely move to buffers; num_dof of btScalar would cover all bodies but acutally 6 btScalars will also be okay
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			invD_times_Y[dof] = 0.f;
+
+			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			{
+				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];				
+			}	
+		}
+		
+		 // Zp += pXi * (Zi + hi*Yi/Di)
+		in_top = zeroAccForce[i+1];            
+        in_bottom = zeroAccTorque[i+1];
+
+		//unroll the loop?
+		for(int row = 0; row < 3; ++row)
+		{
+			for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+			{
+				const btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
+				const btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
+
+				in_top[row] += h_t[row] * invD_times_Y[dof];
+				in_bottom[row] += h_b[row] * invD_times_Y[dof];				
+			}
+		}			
+			
+		InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
+                                in_top, in_bottom, out_top, out_bottom);
+        zeroAccForce[parent+1] += out_top;
+        zeroAccTorque[parent+1] += out_bottom;
+    }
+	
+	// ptr to the joint accel part of the output
+    btScalar * joint_accel = output + 6;
+
+
+    // Second 'upward' loop
+    // (part of TreeForwardDynamics in Mirtich)
+
+    if (m_fixedBase) 
+	{
+        accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
+    } 
+	else 
+	{
+		btVector3 rhs_top (zeroAccForce[0][0], zeroAccForce[0][1], zeroAccForce[0][2]);
+		btVector3 rhs_bot (zeroAccTorque[0][0], zeroAccTorque[0][1], zeroAccTorque[0][2]);
+        float result[6];
+
+		solveImatrix(rhs_top, rhs_bot, result);
+        for (int i = 0; i < 3; ++i) {
+            accel_top[0][i] = -result[i];
+            accel_bottom[0][i] = -result[i+3];
+        }
+
+    }
+
+	static btScalar Y_minus_hT_a[6];					//it's dofx1 for each body so a single 6x1 temp is enough
+    // now do the loop over the m_links
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = m_links[i].m_parent;
+
+		SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
+                         accel_top[parent+1], accel_bottom[parent+1],
+                         accel_top[i+1], accel_bottom[i+1]);		
+		
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			const btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
+			const btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
+
+			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - SpatialDotProduct(h_t, h_b, accel_top[i+1], accel_bottom[i+1]);
+		}
+
+		const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
+		mulMatrix(const_cast<btScalar*>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
+
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			accel_top[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisTop(dof);
+			accel_bottom[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisBottom(dof);
+		}        
+    }
+
+    // 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];
+
+	/////////////////
+	//printf("delta = [");
+	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
+	//	printf("%.2f ", output[dof]);
+	//printf("]\n");
+	/////////////////
+}
+#else				//i.e. TEST_SPATIAL_ALGEBRA_LAYER
+void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
+                                       btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
+{
+    // Temporary matrices/vectors -- use scratch space from caller
+    // so that we don't have to keep reallocating every frame
+
+	int num_links = getNumLinks();	
+    scratch_r.resize(m_dofCount);
+    scratch_v.resize(4*num_links + 4);	    
+
+    btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0;
+    btVector3 * v_ptr = &scratch_v[0];
+
+    // zhat_i^A (scratch space)
+    btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
+	v_ptr += num_links * 2 + 2;
+
+    // rot_from_parent (cached from calcAccelerations)
+    const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
+
+    // hhat (cached), accel (scratch)
+    // hhat is NOT stored for the base (but ahat is) 
+	const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
+	btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
+	v_ptr += num_links * 2 + 2;
+
+    // Y_i (scratch), invD_i (cached)
+    const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
+	btScalar * Y = r_ptr;
+	////////////////
+	//aux variables
+	static btScalar invD_times_Y[6];							//D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
+	static btSpatialMotionVector result;							//holds results of the SolveImatrix op; it is a spatial motion vector (accel)
+	static btScalar Y_minus_hT_a[6];							//Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough	
+	static btSpatialForceVector spatForceVecTemps[6];				//6 temporary spatial force vectors
+	static btSpatialTransformationMatrix fromParent;	
+	/////////////////
+
+    // First 'upward' loop.
+    // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
+	
+	// Fill in zero_acc
+    // -- set to force/torque on the base, zero otherwise
+    if (m_fixedBase) 
+	{
+        zeroAccSpatFrc[0].setZero();
+    } else 
+	{	
+		//test forces
+		fromParent.m_rotMat = rot_from_parent[0];
+		fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]);
+    }
+    for (int i = 0; i < num_links; ++i) 
+	{
+		zeroAccSpatFrc[i+1].setZero();
+    }    
+
+	// 'Downward' loop.
+    // (part of TreeForwardDynamics in Mirtich.)
+    for (int i = num_links - 1; i >= 0; --i)
+	{
+		const int parent = m_links[i].m_parent;
+		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
+
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
+											- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
+											;
+		}
+
+		btVector3 in_top, in_bottom, out_top, out_bottom;
+		const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
+		
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			invD_times_Y[dof] = 0.f;
+
+			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			{
+				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];				
+			}	
+		}
+		
+		 // Zp += pXi * (Zi + hi*Yi/Di)
+		spatForceVecTemps[0] = zeroAccSpatFrc[i+1];
+
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			//
+			spatForceVecTemps[0] += hDof * invD_times_Y[dof];		
+		}
+		
+
+		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
+			
+		zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
+    }
+	
+	// ptr to the joint accel part of the output
+    btScalar * joint_accel = output + 6;
+
+
+    // Second 'upward' loop
+    // (part of TreeForwardDynamics in Mirtich)
+
+    if (m_fixedBase) 
+	{
+        spatAcc[0].setZero();
+    } 
+	else 
+	{
+		solveImatrix(zeroAccSpatFrc[0], result);
+		spatAcc[0] = -result;
+
+    }
+	
+    // now do the loop over the m_links
+    for (int i = 0; i < num_links; ++i)
+	{
+        const int parent = m_links[i].m_parent;
+		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
+
+		fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
+		
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			//			
+			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
+		}
+
+		const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
+		mulMatrix(const_cast<btScalar*>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
+
+		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
+			spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];      
+    }
+
+    // transform base accelerations back to the world frame.
+    btVector3 omegadot_out;
+    omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
+	output[0] = omegadot_out[0];
+	output[1] = omegadot_out[1];
+	output[2] = omegadot_out[2];
+
+    btVector3 vdot_out;
+    vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();
+	output[3] = vdot_out[0];
+	output[4] = vdot_out[1];
+	output[5] = vdot_out[2];
+
+	/////////////////
+	//printf("delta = [");
+	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
+	//	printf("%.2f ", output[dof]);
+	//printf("]\n");
+	/////////////////
+}
+#endif
+
+
+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 = &m_matrixBuf[0];
+
+    // hhat (cached), accel (scratch)
+    const btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0;
+    const btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[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_realBuf[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 (m_fixedBase) 
+	{
+        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) 
+	{
+//		btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
+        Y[i] = - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
+        Y[i] += force[6 + i];  // add joint torque
+        
+        const int parent = m_links[i].m_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], m_links[i].m_cachedRVector,
+                                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 (m_fixedBase) 
+	{
+        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 m_links
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = m_links[i].m_parent;
+        SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
+                         accel_top[parent+1], accel_bottom[parent+1],
+                         accel_top[i+1], accel_bottom[i+1]);
+		btScalar Y_minus_hT_a = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1]));
+        joint_accel[i] = Y_minus_hT_a / D[i];
+        accel_top[i+1] += joint_accel[i] * m_links[i].getAxisTop(0);
+        accel_bottom[i+1] += joint_accel[i] * m_links[i].getAxisBottom(0);
+    }
+
+    // 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];
+
+
+	/////////////////
+	/*
+	int ndof = getNumLinks() + 6;
+	printf("test force(impulse) (%d) = [\n",ndof);
+
+	for (int i=0;i<ndof;i++)
+	{
+			printf("%.2f ", force[i]);
+		printf("]\n");
+	}
+	
+	printf("delta(%d) = [",ndof);
+	for(int dof = 0; dof < getNumLinks() + 6; ++dof)
+		printf("%.2f ", output[dof]);
+	printf("]\n");
+	/////////////////
+*/
+
+	//int dummy = 0;
+}
+
+void btMultiBody::stepPositions(btScalar dt)
+{
+	int num_links = getNumLinks();
+    // step position by adding dt * velocity
+	btVector3 v = getBaseVel();
+    m_basePos += 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|)
+        m_baseQuat = m_baseQuat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term);
+    } else 
+	{
+        m_baseQuat = m_baseQuat * 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.)
+    m_baseQuat.normalize();
+
+    // Finally we can update m_jointPos for each of the m_links
+    for (int i = 0; i < num_links; ++i) 
+	{
+		float jointVel = getJointVel(i);
+        m_links[i].m_jointPos[0] += dt * jointVel;
+        m_links[i].updateCache();
+    }
+}
+
+void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
+{	
+	int num_links = getNumLinks();
+    // step position by adding dt * velocity
+	//btVector3 v = getBaseVel();	
+    //m_basePos += dt * v;
+	//
+	btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
+	btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);			//note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
+	//	
+	pBasePos[0] += dt * pBaseVel[0];
+	pBasePos[1] += dt * pBaseVel[1];
+	pBasePos[2] += dt * pBaseVel[2];
+
+	///////////////////////////////
+	//local functor for quaternion integration (to avoid error prone redundancy)
+	struct
+	{
+		//"exponential map" based on btTransformUtil::integrateTransform(..)
+		void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
+		{
+			//baseBody	=>	quat is alias and omega is global coor
+			//!baseBody	=>	quat is alibi and omega is local coor	
+			
+			btVector3 axis;
+			btVector3 angvel;
+
+			if(!baseBody)			
+				angvel = quatRotate(quat, omega);				//if quat is not m_baseQuat, it is alibi => ok			
+			else
+				angvel = omega;
+		
+			btScalar fAngle = angvel.length(); 		
+			//limit the angular motion
+			if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
+			{
+				fAngle = btScalar(0.5)*SIMD_HALF_PI / dt;
+			}
+
+			if ( fAngle < btScalar(0.001) )
+			{
+				// use Taylor's expansions of sync function
+				axis   = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle );
+			}
+			else
+			{
+				// sync(fAngle) = sin(c*fAngle)/t
+				axis   = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle );
+			}
+			
+			if(!baseBody)				
+				quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat;			
+			else			
+				quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) ));
+				//equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();			
+		
+			quat.normalize();
+		}
+	} pQuatUpdateFun;
+	///////////////////////////////
+
+	//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
+	//	
+	btScalar *pBaseQuat = pq ? pq : m_baseQuat;	
+	btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];		//note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
+	//
+	static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+	static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+	pQuatUpdateFun(baseOmega, baseQuat, true, dt);
+	pBaseQuat[0] = baseQuat.x();
+	pBaseQuat[1] = baseQuat.y();
+	pBaseQuat[2] = baseQuat.z();
+	pBaseQuat[3] = baseQuat.w();
+
+
+	//printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
+	//printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
+	//printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
+
+	if(pq)		
+		pq += 7;
+	if(pqd)
+		pqd += 6;
+
+	// Finally we can update m_jointPos for each of the m_links
+    for (int i = 0; i < num_links; ++i) 
+	{
+		btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);		
+		btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
+
+		switch(m_links[i].m_jointType)
+		{
+			case btMultibodyLink::ePrismatic:
+			case btMultibodyLink::eRevolute:
+			{
+				btScalar jointVel = pJointVel[0];	
+				pJointPos[0] += dt * jointVel;
+				break;
+			}
+			case btMultibodyLink::eSpherical:
+			{
+				static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+				static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+				pQuatUpdateFun(jointVel, jointOri, false, dt);
+				pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
+				break;
+			}
+#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+			case btMultibodyLink::ePlanar:
+			{
+				pJointPos[0] += dt * getJointVelMultiDof(i)[0];
+
+				btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
+				btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
+				pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
+				pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
+
+				break;
+			}
+#endif
+			default:
+			{
+			}
+
+		}
+
+		m_links[i].updateCacheMultiDof(pq);
+
+		if(pq)		
+			pq += m_links[i].m_posVarCount;
+		if(pqd)
+			pqd += m_links[i].m_dofCount;
+    }
+}
+
+void btMultiBody::filConstraintJacobianMultiDof(int link,
+                                    const btVector3 &contact_point,
+									const btVector3 &normal_ang,
+                                    const btVector3 &normal_lin,
+                                    btScalar *jac,
+                                    btAlignedObjectArray<btScalar> &scratch_r,
+                                    btAlignedObjectArray<btVector3> &scratch_v,
+                                    btAlignedObjectArray<btMatrix3x3> &scratch_m) const
+{
+    // temporary space
+	int num_links = getNumLinks();
+	int m_dofCount = getNumDofs();
+    scratch_v.resize(3*num_links + 3);			//(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
+    scratch_m.resize(num_links + 1);
+
+    btVector3 * v_ptr = &scratch_v[0];
+    btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
+    btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
+	btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
+    btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
+
+    scratch_r.resize(m_dofCount);
+    btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
+
+    btMatrix3x3 * rot_from_world = &scratch_m[0];
+
+    const btVector3 p_minus_com_world = contact_point - m_basePos;
+	const btVector3 &normal_lin_world = normal_lin;							//convenience
+	const btVector3 &normal_ang_world = normal_ang;
+
+    rot_from_world[0] = btMatrix3x3(m_baseQuat);    
+    
+    // omega coeffients first.
+    btVector3 omega_coeffs_world;
+    omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
+	jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
+	jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
+	jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
+    // then v coefficients
+    jac[3] = normal_lin_world[0];
+    jac[4] = normal_lin_world[1];
+    jac[5] = normal_lin_world[2];
+
+	//create link-local versions of p_minus_com and normal
+	p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
+    n_local_lin[0] = rot_from_world[0] * normal_lin_world;
+	n_local_ang[0] = rot_from_world[0] * normal_ang_world;
+
+    // Set remaining jac values to zero for now.
+    for (int i = 6; i < 6 + m_dofCount; ++i) 
+	{
+        jac[i] = 0;
+    }
+
+    // Qdot coefficients, if necessary.
+    if (num_links > 0 && link > -1) {
+
+        // TODO: speed this up -- don't calculate for m_links we don't need.
+        // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
+        // which is resulting in repeated work being done...)
+
+        // calculate required normals & positions in the local frames.
+        for (int i = 0; i < num_links; ++i) {
+
+            // transform to local frame
+            const int parent = m_links[i].m_parent;
+            const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
+            rot_from_world[i+1] = mtx * rot_from_world[parent+1];
+
+            n_local_lin[i+1] = mtx * n_local_lin[parent+1];
+			n_local_ang[i+1] = mtx * n_local_ang[parent+1];
+            p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector;
+
+			// calculate the jacobian entry
+			switch(m_links[i].m_jointType)
+			{
+				case btMultibodyLink::eRevolute:
+				{
+					results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
+					results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
+					break;
+				}
+				case btMultibodyLink::ePrismatic:
+				{
+					results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
+					break;
+				}
+				case btMultibodyLink::eSpherical:
+				{
+					results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
+					results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1));
+					results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2));
+										
+					results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
+					results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1));
+					results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2));
+
+					break;
+				}
+#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+				case btMultibodyLink::ePlanar:
+				{
+					results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
+					results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
+					results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
+
+					break;
+				}
+#endif
+				default:
+				{
+				}
+			}
+            
+        }
+
+        // Now copy through to output.
+		//printf("jac[%d] = ", link);
+        while (link != -1) 
+		{
+			for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
+			{
+				jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
+				//printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
+			}
+            
+			link = m_links[link].m_parent;
+        }
+		//printf("]\n");
+    }
+}
+
+void btMultiBody::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 - m_basePos;
+
+    rot_from_world[0] = btMatrix3x3(m_baseQuat);
+
+    p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
+    n_local[0] = rot_from_world[0] * normal;
+    
+    // omega coeffients first.
+	if (this->m_fixedBase)
+	{
+		for (int i=0;i<6;i++)
+		{
+			jac[i]=0;
+		}
+	} else
+	{
+		    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 m_links we don't need.
+        // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
+        // which is resulting in repeated work being done...)
+
+        // calculate required normals & positions in the local frames.
+        for (int i = 0; i < num_links; ++i) {
+
+            // transform to local frame
+            const int parent = m_links[i].m_parent;
+            const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
+            rot_from_world[i+1] = mtx * rot_from_world[parent+1];
+
+            n_local[i+1] = mtx * n_local[parent+1];
+            p_minus_com[i+1] = mtx * p_minus_com[parent+1] - m_links[i].m_cachedRVector;
+
+            // calculate the jacobian entry
+			if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
+                results[i] = n_local[i+1].dot( m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0) );
+            } else {
+                results[i] = n_local[i+1].dot( m_links[i].getAxisBottom(0) );
+            }
+        }
+
+        // Now copy through to output.
+		//printf("jac[%d] = ", link);
+        while (link != -1) 
+		{
+            jac[6 + link] = results[link];
+			//printf("%.2f\t", jac[6 + link]);
+            link = m_links[link].m_parent;
+        }
+		//printf("]\n");
+    }
+}
+
+void btMultiBody::wakeUp()
+{
+    m_awake = true;
+}
+
+void btMultiBody::goToSleep()
+{
+    m_awake = false;
+}
+
+void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
+{
+	int num_links = getNumLinks();
+	extern bool gDisableDeactivation;
+    if (!m_canSleep || gDisableDeactivation) 
+	{
+		m_awake = true;
+		m_sleepTimer = 0;
+		return;
+	}
+
+    // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
+    btScalar motion = 0;
+	if(m_isMultiDof)
+	{
+		for (int i = 0; i < 6 + m_dofCount; ++i) 		
+			motion += m_realBuf[i] * m_realBuf[i];
+	}
+	else
+	{
+		for (int i = 0; i < 6 + num_links; ++i) 		
+			motion += m_realBuf[i] * m_realBuf[i];		
+	}  
+    
+
+    if (motion < SLEEP_EPSILON) {
+        m_sleepTimer += timestep;
+        if (m_sleepTimer > SLEEP_TIMEOUT) {
+            goToSleep();
+        }
+    } else {
+        m_sleepTimer = 0;
+		if (!m_awake)
+			wakeUp();
+    }
+}

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

@@ -1,466 +1,620 @@
-/*
- * 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
+/*
+ * 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 fixedBase,           // whether the base is fixed (true) or can move (false)
+		bool canSleep,
+		bool multiDof = false
+			  );
+
+
+    ~btMultiBody();	
+    
+	void setupFixed(int linkIndex,
+						   btScalar mass,
+						   const btVector3 &inertia,
+						   int parent,
+						   const btQuaternion &rotParentToThis,
+						   const btVector3 &parentComToThisPivotOffset,
+                           const btVector3 &thisPivotToThisComOffset,
+						   bool disableParentCollision);
+
+						
+	void setupPrismatic(int i,
+                               btScalar mass,
+                               const btVector3 &inertia,
+                               int parent,
+                               const btQuaternion &rotParentToThis,
+                               const btVector3 &jointAxis,
+                               const btVector3 &parentComToThisComOffset,
+							   const btVector3 &thisPivotToThisComOffset,
+							   bool disableParentCollision);
+
+    void setupRevolute(int linkIndex,            // 0 to num_links-1
+                       btScalar mass,
+                       const btVector3 &inertia,
+                       int parentIndex,
+                       const btQuaternion &rotParentToThis,  // rotate points in parent frame to this frame, when q = 0
+                       const btVector3 &jointAxis,    // in my frame
+                       const btVector3 &parentComToThisPivotOffset,    // vector from parent COM to joint axis, in PARENT frame
+                       const btVector3 &thisPivotToThisComOffset,       // vector from joint axis to my COM, in MY frame
+					   bool disableParentCollision=false);
+
+	void setupSpherical(int linkIndex,											// 0 to num_links-1
+                       btScalar mass,
+                       const btVector3 &inertia,
+                       int parent,
+                       const btQuaternion &rotParentToThis,		// rotate points in parent frame to this frame, when q = 0                       
+                       const btVector3 &parentComToThisPivotOffset,			// vector from parent COM to joint axis, in PARENT frame
+                       const btVector3 &thisPivotToThisComOffset,				// vector from joint axis to my COM, in MY frame
+					   bool disableParentCollision=false);		
+
+#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+	void setupPlanar(int i,											// 0 to num_links-1
+                       btScalar mass,
+                       const btVector3 &inertia,
+                       int parent,
+                       const btQuaternion &rotParentToThis,		// rotate points in parent frame to this frame, when q = 0                       
+					   const btVector3 &rotationAxis,
+                       const btVector3 &parentComToThisComOffset,			// vector from parent COM to this COM, in PARENT frame                       
+					   bool disableParentCollision=false);		
+#endif
+	
+	const btMultibodyLink& getLink(int index) const
+	{
+		return m_links[index];
+	}
+
+	btMultibodyLink& getLink(int index)
+	{
+		return m_links[index];
+	}
+
+
+	void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base
+	{
+		m_baseCollider = collider;
+	}
+	const btMultiBodyLinkCollider* getBaseCollider() const
+	{
+		return m_baseCollider;
+	}
+	btMultiBodyLinkCollider* getBaseCollider()
+	{
+		return m_baseCollider;
+	}
+
+    //
+    // get parent
+    // input: link num from 0 to num_links-1
+    // output: link num from 0 to num_links-1, OR -1 to mean the base.
+    //
+    int getParent(int link_num) const;
+    
+    
+    //
+    // get number of m_links, masses, moments of inertia
+    //
+
+    int getNumLinks() const { return m_links.size(); }
+	int getNumDofs() const { return m_dofCount; }
+	int getNumPosVars() const { return m_posVarCnt; }
+    btScalar getBaseMass() const { return m_baseMass; }
+    const btVector3 & getBaseInertia() const { return m_baseInertia; }
+    btScalar getLinkMass(int i) const;
+    const btVector3 & getLinkInertia(int i) const;
+    
+
+    //
+    // change mass (incomplete: can only change base mass and inertia at present)
+    //
+
+    void setBaseMass(btScalar mass) { m_baseMass = mass; }
+    void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; }
+
+
+    //
+    // get/set pos/vel/rot/omega for the base link
+    //
+
+    const btVector3 & getBasePos() const { return m_basePos; }    // in world frame
+    const btVector3 getBaseVel() const 
+	{ 
+		return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]); 
+	}     // in world frame
+    const btQuaternion & getWorldToBaseRot() const 
+	{ 
+		return m_baseQuat; 
+	}     // rotates world vectors into base frame
+    btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); }   // in world frame
+
+    void setBasePos(const btVector3 &pos) 
+	{ 
+		m_basePos = pos; 
+	}
+    void setBaseVel(const btVector3 &vel) 
+	{ 
+
+		m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2]; 
+	}
+    void setWorldToBaseRot(const btQuaternion &rot) 
+	{ 
+		m_baseQuat = rot;					//m_baseQuat asumed to ba alias!?
+	}
+    void setBaseOmega(const btVector3 &omega) 
+	{ 
+		m_realBuf[0]=omega[0]; 
+		m_realBuf[1]=omega[1]; 
+		m_realBuf[2]=omega[2]; 
+	}
+
+
+    //
+    // get/set pos/vel for child m_links (i = 0 to num_links-1)
+    //
+
+    btScalar getJointPos(int i) const;
+    btScalar getJointVel(int i) const;
+
+	btScalar * getJointVelMultiDof(int i);
+	btScalar * getJointPosMultiDof(int i);
+
+    void setJointPos(int i, btScalar q);
+    void setJointVel(int i, btScalar qdot);
+	void setJointPosMultiDof(int i, btScalar *q);
+    void setJointVelMultiDof(int i, btScalar *qdot);	
+
+    //
+    // direct access to velocities as a vector of 6 + num_links elements.
+    // (omega first, then v, then joint velocities.)
+    //
+    const btScalar * getVelocityVector() const 
+	{ 
+		return &m_realBuf[0]; 
+	}
+/*    btScalar * getVelocityVector() 
+	{ 
+		return &real_buf[0]; 
+	}
+  */  
+
+    //
+    // get the frames of reference (positions and orientations) of the child m_links
+    // (i = 0 to num_links-1)
+    //
+
+    const btVector3 & getRVector(int i) const;   // vector from COM(parent(i)) to COM(i), in frame i's coords
+    const btQuaternion & getParentToLocalRot(int i) const;   // rotates vectors in frame parent(i) to vectors in frame i.
+
+
+    //
+    // transform vectors in local frame of link i to world frame (or vice versa)
+    //
+    btVector3 localPosToWorld(int i, const btVector3 &vec) const;
+    btVector3 localDirToWorld(int i, const btVector3 &vec) const;
+    btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
+    btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
+    
+
+    //
+    // 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) 
+	{ 
+		m_baseForce += f; 
+	}
+    void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
+    void addLinkForce(int i, const btVector3 &f);
+    void addLinkTorque(int i, const btVector3 &t);
+    void addJointTorque(int i, btScalar Q);
+	void addJointTorqueMultiDof(int i, int dof, btScalar Q);
+	void addJointTorqueMultiDof(int i, const btScalar *Q);
+
+    const btVector3 & getBaseForce() const { return m_baseForce; }
+    const btVector3 & getBaseTorque() const { return m_baseTorque; }
+    const btVector3 & getLinkForce(int i) const;
+    const btVector3 & getLinkTorque(int i) const;
+    btScalar getJointTorque(int i) const;
+	btScalar * getJointTorqueMultiDof(int i);
+
+
+    //
+    // dynamics routines.
+    //
+
+    // timestep the velocities (given the external forces/torques set using addBaseForce etc).
+    // also sets up caches for calcAccelerationDeltas.
+    //
+    // Note: the caller must provide three vectors which are used as
+    // temporary scratch space. The idea here is to reduce dynamic
+    // memory allocation: the same scratch vectors can be re-used
+    // again and again for different Multibodies, instead of each
+    // btMultiBody allocating (and then deallocating) their own
+    // individual scratch buffers. This gives a considerable speed
+    // improvement, at least on Windows (where dynamic memory
+    // allocation appears to be fairly slow).
+    //
+    void stepVelocities(btScalar dt,
+                        btAlignedObjectArray<btScalar> &scratch_r,
+                        btAlignedObjectArray<btVector3> &scratch_v,
+                        btAlignedObjectArray<btMatrix3x3> &scratch_m);
+
+	void stepVelocitiesMultiDof(btScalar dt,
+                        btAlignedObjectArray<btScalar> &scratch_r,
+                        btAlignedObjectArray<btVector3> &scratch_v,
+                        btAlignedObjectArray<btMatrix3x3> &scratch_m);
+
+    // calcAccelerationDeltas
+    // input: force vector (in same format as jacobian, i.e.:
+    //                      3 torque values, 3 force values, num_links joint torque values)
+    // 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;
+
+	void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
+                                btAlignedObjectArray<btScalar> &scratch_r,
+                                btAlignedObjectArray<btVector3> &scratch_v) const;
+
+    // apply a delta-vee directly. used in sequential impulses code.
+    void applyDeltaVee(const btScalar * delta_vee) 
+	{
+
+        for (int i = 0; i < 6 + getNumLinks(); ++i) 
+		{
+			m_realBuf[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_realBuf[i] += delta_vee[i] * multiplier;
+			btClamp(m_realBuf[i],-m_maxCoordinateVelocity,m_maxCoordinateVelocity);
+		}
+    }
+
+	void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier) 
+	{
+		//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+		//	printf("%.4f ", delta_vee[dof]*multiplier);
+		//printf("\n");
+
+		//btScalar sum = 0;
+		//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+		//{
+		//	sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
+		//}
+		//btScalar l = btSqrt(sum);
+
+		//if (l>m_maxAppliedImpulse)
+		//{
+		//	multiplier *= m_maxAppliedImpulse/l;
+		//}
+
+		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+		{
+			m_realBuf[dof] += delta_vee[dof] * multiplier;
+			btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity);
+		}
+    }
+
+    // timestep the positions (given current velocities).
+    void stepPositions(btScalar dt);
+	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
+
+
+    //
+    // contacts
+    //
+
+    // This routine fills out a contact constraint jacobian for this body.
+    // the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
+    // 'normal' & 'contact_point' are both given in world coordinates.
+    void 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;
+
+	//multidof version of fillContactJacobian
+	void fillContactJacobianMultiDof(int link,
+                             const btVector3 &contact_point,
+                             const btVector3 &normal,
+                             btScalar *jac,
+                             btAlignedObjectArray<btScalar> &scratch_r,
+                             btAlignedObjectArray<btVector3> &scratch_v,
+							 btAlignedObjectArray<btMatrix3x3> &scratch_m) const { filConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
+
+	//a more general version of fillContactJacobianMultiDof which does not assume..
+	//.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
+	void filConstraintJacobianMultiDof(int link,
+                             const btVector3 &contact_point,
+							 const btVector3 &normal_ang,
+                             const btVector3 &normal_lin,
+                             btScalar *jac,
+                             btAlignedObjectArray<btScalar> &scratch_r,
+                             btAlignedObjectArray<btVector3> &scratch_v,
+                             btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
+
+
+    //
+    // sleeping
+    //
+	void	setCanSleep(bool canSleep)
+	{
+		m_canSleep = canSleep;
+	}
+
+	bool getCanSleep()const
+	{
+		return m_canSleep;
+	}
+
+    bool isAwake() const { return m_awake; }
+    void wakeUp();
+    void goToSleep();
+    void checkMotionAndSleepIfRequired(btScalar timestep);
+    
+	bool hasFixedBase() const
+	{
+		    return m_fixedBase;
+	}
+
+	int getCompanionId() const
+	{
+		return m_companionId;
+	}
+	void setCompanionId(int id)
+	{
+		//printf("for %p setCompanionId(%d)\n",this, id);
+		m_companionId = id;
+	}
+
+	void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
+	{
+		m_links.resize(numLinks);
+	}
+
+	btScalar getLinearDamping() const
+	{
+			return m_linearDamping;
+	}
+	void setLinearDamping( btScalar damp)
+	{
+		m_linearDamping = damp;
+	}
+	btScalar getAngularDamping() const
+	{
+		return m_angularDamping;
+	}
+	void setAngularDamping( btScalar damp)
+	{
+		m_angularDamping = damp;
+	}
+		
+	bool getUseGyroTerm() const
+	{
+		return m_useGyroTerm;
+	}
+	void setUseGyroTerm(bool useGyro)
+	{
+		m_useGyroTerm = useGyro;
+	}
+	btScalar	getMaxCoordinateVelocity() const
+	{
+		return m_maxCoordinateVelocity ;
+	}
+	void	setMaxCoordinateVelocity(btScalar maxVel)
+	{
+		m_maxCoordinateVelocity = maxVel;
+	}
+
+	btScalar	getMaxAppliedImpulse() const
+	{
+		return m_maxAppliedImpulse;
+	}
+	void	setMaxAppliedImpulse(btScalar maxImp)
+	{
+		m_maxAppliedImpulse = maxImp;
+	}
+	void	setHasSelfCollision(bool hasSelfCollision)
+	{
+		m_hasSelfCollision = hasSelfCollision;
+	}
+	bool hasSelfCollision() const
+	{
+		return m_hasSelfCollision;
+	}
+
+	bool isMultiDof() { return m_isMultiDof; }
+	void finalizeMultiDof();
+
+	void useRK4Integration(bool use) { m_useRK4 = use; }
+	bool isUsingRK4Integration() const { return m_useRK4; }
+	void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
+	bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; }
+
+	bool isPosUpdated() const
+	{
+		return __posUpdated;
+	}
+	void setPosUpdated(bool updated)
+	{
+		__posUpdated = updated;
+	}
+	
+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;
+#ifdef TEST_SPATIAL_ALGEBRA_LAYER
+	void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
+#endif
+	
+	void updateLinksDofOffsets()
+	{
+		int dofOffset = 0, cfgOffset = 0;
+		for(int bidx = 0; bidx < m_links.size(); ++bidx)
+		{
+			m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset;
+			dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount;
+		}
+	}
+
+	void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
+	
+	
+private:
+
+	btMultiBodyLinkCollider* m_baseCollider;//can be NULL
+
+    btVector3 m_basePos;       // position of COM of base (world frame)
+    btQuaternion m_baseQuat;   // rotates world points into base frame
+
+    btScalar m_baseMass;         // mass of the base
+    btVector3 m_baseInertia;   // inertia of the base (in local frame; diagonal)
+
+    btVector3 m_baseForce;     // external force applied to base. World frame.
+    btVector3 m_baseTorque;    // external torque applied to base. World frame.
+    
+    btAlignedObjectArray<btMultibodyLink> m_links;    // array of m_links, excluding the base. index from 0 to num_links-1.
+	btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
+
+    
+    //
+    // realBuf:
+    //  offset         size            array
+    //   0              6 + num_links   v (base_omega; base_vel; joint_vels)					MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized]
+    //   6+num_links    num_links       D
+    //
+    // vectorBuf:
+    //  offset         size         array
+    //   0              num_links    h_top
+    //   num_links      num_links    h_bottom
+    //
+    // matrixBuf:
+    //  offset         size         array
+    //   0              num_links+1  rot_from_parent
+    //
+    
+    btAlignedObjectArray<btScalar> m_realBuf;
+    btAlignedObjectArray<btVector3> m_vectorBuf;
+    btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
+
+    //std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
+
+	btMatrix3x3 m_cachedInertiaTopLeft;
+	btMatrix3x3 m_cachedInertiaTopRight;
+	btMatrix3x3 m_cachedInertiaLowerLeft;
+	btMatrix3x3 m_cachedInertiaLowerRight;
+
+    bool m_fixedBase;
+
+    // Sleep parameters.
+    bool m_awake;
+    bool m_canSleep;
+    btScalar m_sleepTimer;
+
+	int	m_companionId;
+	btScalar	m_linearDamping;
+	btScalar	m_angularDamping;
+	bool	m_useGyroTerm;
+	btScalar	m_maxAppliedImpulse;
+	btScalar	m_maxCoordinateVelocity;
+	bool		m_hasSelfCollision;
+	bool		m_isMultiDof;
+		bool __posUpdated;
+		int m_dofCount, m_posVarCnt;
+	bool m_useRK4, m_useGlobalVelocities;
+};
+
+#endif

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

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

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

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

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

@@ -36,6 +36,10 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 		//if (iteration < constraint.m_overrideNumSolverIterations)
 			//resolveSingleConstraintRowGenericMultiBody(constraint);
 		resolveSingleConstraintRowGeneric(constraint);
+		if(constraint.m_multiBodyA) 
+			constraint.m_multiBodyA->setPosUpdated(false);
+		if(constraint.m_multiBodyB) 
+			constraint.m_multiBodyB->setPosUpdated(false);
 	}
 
 	//solve featherstone normal contact
@@ -44,6 +48,11 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 		btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
 		if (iteration < infoGlobal.m_numIterations)
 			resolveSingleConstraintRowGeneric(constraint);
+
+		if(constraint.m_multiBodyA) 
+			constraint.m_multiBodyA->setPosUpdated(false);
+		if(constraint.m_multiBodyB) 
+			constraint.m_multiBodyB->setPosUpdated(false);
 	}
 	
 	//solve featherstone frictional contact
@@ -60,6 +69,11 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 				frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
 				frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
 				resolveSingleConstraintRowGeneric(frictionConstraint);
+
+				if(frictionConstraint.m_multiBodyA) 
+					frictionConstraint.m_multiBodyA->setPosUpdated(false);
+				if(frictionConstraint.m_multiBodyB) 
+					frictionConstraint.m_multiBodyB->setPosUpdated(false);
 			}
 		}
 	}
@@ -108,10 +122,10 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 
 	if (c.m_multiBodyA)
 	{
-		ndofA  = c.m_multiBodyA->getNumLinks() + 6;
+		ndofA  = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6;
 		for (int i = 0; i < ndofA; ++i) 
 			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
-	} else
+	} else if(c.m_solverBodyIdA >= 0)
 	{
 		bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
 		deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
@@ -119,10 +133,10 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 
 	if (c.m_multiBodyB)
 	{
-		ndofB = c.m_multiBodyB->getNumLinks() + 6;
+		ndofB  = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6;
 		for (int i = 0; i < ndofB; ++i) 
 			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
-	} else
+	} else if(c.m_solverBodyIdB >= 0)
 	{
 		bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
 		deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
@@ -151,8 +165,13 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 	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
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		if(c.m_multiBodyA->isMultiDof())
+			c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
+		else
+			c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
+	} else if(c.m_solverBodyIdA >= 0)
 	{
 		bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
 
@@ -160,8 +179,13 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 	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
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		if(c.m_multiBodyB->isMultiDof())
+			c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
+		else
+			c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
+	} else if(c.m_solverBodyIdB >= 0)
 	{
 		bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
 	}
@@ -180,14 +204,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(con
 
 	if (c.m_multiBodyA)
 	{
-		ndofA  = c.m_multiBodyA->getNumLinks() + 6;
+		ndofA  = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6;
 		for (int i = 0; i < ndofA; ++i) 
 			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
 	}
 
 	if (c.m_multiBodyB)
 	{
-		ndofB = c.m_multiBodyB->getNumLinks() + 6;
+		ndofB  = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6;
 		for (int i = 0; i < ndofB; ++i) 
 			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
 	}
@@ -257,7 +281,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 	if (multiBodyA)
 	{
-		const int ndofA  = multiBodyA->getNumLinks() + 6;
+		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 
 		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 
@@ -277,9 +301,15 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		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);
+		if(multiBodyA->isMultiDof())
+			multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+		else
+			multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
 		btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-		multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
+		if(multiBodyA->isMultiDof())
+			multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
+		else
+			multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
 	} else
 	{
 		btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
@@ -290,7 +320,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 	if (multiBodyB)
 	{
-		const int ndofB  = multiBodyB->getNumLinks() + 6;
+		const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 
 		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
 		if (solverConstraint.m_deltaVelBindex <0)
@@ -306,8 +336,14 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
 		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 
-		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);
+		if(multiBodyB->isMultiDof())
+			multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+		else
+			multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+		if(multiBodyB->isMultiDof())
+			multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
+		else
+			multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
 	} else
 	{
 		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);		
@@ -328,7 +364,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		int ndofA  = 0;
 		if (multiBodyA)
 		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
+			ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 			jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 			lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
@@ -347,7 +383,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 		if (multiBodyB)
 		{
-			const int ndofB  = multiBodyB->getNumLinks() + 6;
+			const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 			jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
 			lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
@@ -366,24 +402,16 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			}
 		}
 
-		 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)
+		 if (d>SIMD_EPSILON)
 		 {
-			 
-			 solverConstraint.m_jacDiagABInv = relaxation/(d);
+			solverConstraint.m_jacDiagABInv = relaxation/(d);
 		 } else
 		 {
-			solverConstraint.m_jacDiagABInv  = 1.f;
+			//disable the constraint row to handle singularity/redundant constraint
+			solverConstraint.m_jacDiagABInv  = 0.f;
 		 }
 		
 	}
@@ -404,7 +432,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		btVector3 vel1,vel2;
 		if (multiBodyA)
 		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
+			ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 			btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA ; ++i) 
 				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
@@ -417,7 +445,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 		if (multiBodyB)
 		{
-			ndofB  = multiBodyB->getNumLinks() + 6;
+			ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 			btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB ; ++i) 
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
@@ -432,17 +460,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 		solverConstraint.m_friction = cp.m_combinedFriction;
 
-				
-		restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
-		if (restitution <= btScalar(0.))
+		if(!isFriction)
 		{
-			restitution = 0.f;
-		};
+			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)
+	//disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
+	if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 	{
 		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
 
@@ -452,7 +483,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			{
 				btScalar impulse = solverConstraint.m_appliedImpulse;
 				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-				multiBodyA->applyDeltaVee(deltaV,impulse);
+				if(multiBodyA->isMultiDof())
+					multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
+				else
+					multiBodyA->applyDeltaVee(deltaV,impulse);
 				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
 			} else
 			{
@@ -463,7 +497,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			{
 				btScalar impulse = solverConstraint.m_appliedImpulse;
 				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-				multiBodyB->applyDeltaVee(deltaV,impulse);
+				if(multiBodyB->isMultiDof())
+					multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
+				else
+					multiBodyB->applyDeltaVee(deltaV,impulse);
 				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
 			} else
 			{
@@ -479,11 +516,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 	solverConstraint.m_appliedPushImpulse = 0.f;
 
 	{
-		
 
 		btScalar positionalError = 0.f;
-		btScalar	velocityError = restitution - rel_vel;// * damping;
-					
+		btScalar velocityError = restitution - rel_vel;// * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
 
 		btScalar erp = infoGlobal.m_erp2;
 		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
@@ -494,7 +529,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		if (penetration>0)
 		{
 			positionalError = 0;
-			velocityError = -penetration / infoGlobal.m_timeStep;
+			velocityError -= penetration / infoGlobal.m_timeStep;
 
 		} else
 		{
@@ -504,22 +539,33 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
 		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
 
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		if(!isFriction)
 		{
-			//combine position and velocity into rhs
-			solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
-			solverConstraint.m_rhsPenetration = 0.f;
+			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
+			} else
+			{
+				//split position and velocity into rhs and m_rhsPenetration
+				solverConstraint.m_rhs = velocityImpulse;
+				solverConstraint.m_rhsPenetration = penetrationImpulse;
+			}
+
+			solverConstraint.m_lowerLimit = 0;
+			solverConstraint.m_upperLimit = 1e10f;
+		}
+		else
 		{
-			//split position and velocity into rhs and m_rhsPenetration
 			solverConstraint.m_rhs = velocityImpulse;
-			solverConstraint.m_rhsPenetration = penetrationImpulse;
+			solverConstraint.m_rhsPenetration = 0.f;
+			solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
+			solverConstraint.m_upperLimit = solverConstraint.m_friction;
 		}
 
-		solverConstraint.m_cfm = 0.f;
-		solverConstraint.m_lowerLimit = 0;
-		solverConstraint.m_upperLimit = 1e10f;
+		solverConstraint.m_cfm = 0.f;			//why not use cfmSlip?
 	}
 
 }
@@ -575,15 +621,15 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 	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];
+//	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++)
 	{
@@ -599,8 +645,8 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 
 			btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
 
-			btRigidBody* rb0 = btRigidBody::upcast(colObj0);
-			btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+	//		btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+	//		btRigidBody* rb1 = btRigidBody::upcast(colObj1);
 			solverConstraint.m_solverBodyIdA = solverBodyIdA;
 			solverConstraint.m_solverBodyIdB = solverBodyIdB;
 			solverConstraint.m_multiBodyA = mbA;
@@ -624,6 +670,7 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 #ifdef ENABLE_FRICTION
 			solverConstraint.m_frictionIndex = frictionIndex;
 #if ROLLING_FRICTION
+	int rollingFriction=1;
 			btVector3 angVelA(0,0,0),angVelB(0,0,0);
 			if (rb0)
 				angVelA = rb0->getAngularVelocity();
@@ -702,6 +749,10 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 				{
 					btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
 
+					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
+
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
@@ -709,10 +760,6 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 						addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
 					}
 
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
-
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
 					{
 						cp.m_lateralFrictionInitialized = true;
@@ -741,7 +788,7 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 
 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
 {
-	btPersistentManifold* manifold = 0;
+	//btPersistentManifold* manifold = 0;
 
 	for (int i=0;i<numManifolds;i++)
 	{
@@ -779,6 +826,63 @@ btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int
 	return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
 }
 
+btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
+{
+	int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
+	int j;
+
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		for (j=0;j<numPoolConstraints;j++)
+		{
+			const btMultiBodySolverConstraint& solveManifold = m_multiBodyNormalContactConstraints[j];
+			btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
+			btAssert(pt);
+			pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
+		
+			pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex].m_appliedImpulse;
+			//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
+			if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+			{
+				pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex+1].m_appliedImpulse;
+			}
+			//do a callback here?
+		}
+	}
+	
+
+	numPoolConstraints = m_multiBodyNonContactConstraints.size();
+
+#if 0
+	//@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
+	for (int i=0;i<numPoolConstraints;i++)
+	{
+		const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
+
+		btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
+		btJointFeedback* fb = constr->getJointFeedback();
+		if (fb)
+		{
+			fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
+			fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
+			fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
+			fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
+			
+		}
+
+		constr->internalSetAppliedImpulse(c.m_appliedImpulse);
+		if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
+		{
+			constr->setEnabled(false);
+		}
+
+	}
+#endif 
+
+
+	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
+}
+
 
 void  btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
 {

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

@@ -73,7 +73,8 @@ public:
 
 	///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
 	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
+	
 	virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
 };
 

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

@@ -20,7 +20,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
 #include "LinearMath/btQuickprof.h"
 #include "btMultiBodyConstraint.h"
-
+#include "LinearMath/btIDebugDraw.h"
 	
 
 
@@ -364,7 +364,9 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 		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;
+		btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;			
+
+		//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
 	
 		m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
 		m_bodies.resize(0);
@@ -392,9 +394,6 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
 	delete m_solverMultiBodyIslandCallback;
 }
 
-
-
-
 void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
 
@@ -451,11 +450,11 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 
 			if (!isSleeping)
 			{
-				scratch_r.resize(bod->getNumLinks()+1);
+				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+				scratch_r.resize(bod->getNumLinks()+1);			//multidof? ("Y"s use it and it is used to store qdd)
 				scratch_v.resize(bod->getNumLinks()+1);
 				scratch_m.resize(bod->getNumLinks()+1);
 
-				bod->clearForcesAndTorques();
 				bod->addBaseForce(m_gravity * bod->getBaseMass());
 
 				for (int j = 0; j < bod->getNumLinks(); ++j) 
@@ -463,8 +462,176 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 					bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
 				}
 
-				bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
-			}
+				bool doNotUpdatePos = false;
+
+				if(bod->isMultiDof())
+				{
+					if(!bod->isUsingRK4Integration())
+					{
+						bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
+					}
+					else
+					{						
+						//
+						int numDofs = bod->getNumDofs() + 6;
+						int numPosVars = bod->getNumPosVars() + 7;
+						btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
+						//convenience
+						btScalar *pMem = &scratch_r2[0];
+						btScalar *scratch_q0 = pMem; pMem += numPosVars;
+						btScalar *scratch_qx = pMem; pMem += numPosVars;
+						btScalar *scratch_qd0 = pMem; pMem += numDofs;
+						btScalar *scratch_qd1 = pMem; pMem += numDofs;
+						btScalar *scratch_qd2 = pMem; pMem += numDofs;
+						btScalar *scratch_qd3 = pMem; pMem += numDofs;
+						btScalar *scratch_qdd0 = pMem; pMem += numDofs;
+						btScalar *scratch_qdd1 = pMem; pMem += numDofs;
+						btScalar *scratch_qdd2 = pMem; pMem += numDofs;
+						btScalar *scratch_qdd3 = pMem; pMem += numDofs;
+						btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
+
+						/////						
+						//copy q0 to scratch_q0 and qd0 to scratch_qd0
+						scratch_q0[0] = bod->getWorldToBaseRot().x();
+						scratch_q0[1] = bod->getWorldToBaseRot().y();
+						scratch_q0[2] = bod->getWorldToBaseRot().z();
+						scratch_q0[3] = bod->getWorldToBaseRot().w();
+						scratch_q0[4] = bod->getBasePos().x();
+						scratch_q0[5] = bod->getBasePos().y();
+						scratch_q0[6] = bod->getBasePos().z();
+						//
+						for(int link = 0; link < bod->getNumLinks(); ++link)
+						{
+							for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
+								scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];							
+						}
+						//
+						for(int dof = 0; dof < numDofs; ++dof)								
+							scratch_qd0[dof] = bod->getVelocityVector()[dof];
+						////
+						struct
+						{
+						    btMultiBody *bod;
+                            btScalar *scratch_qx, *scratch_q0;
+
+						    void operator()()
+						    {
+						        for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
+                                    scratch_qx[dof] = scratch_q0[dof];
+						    }
+						} pResetQx = {bod, scratch_qx, scratch_q0};
+						//
+						struct
+						{
+						    void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
+						    {
+						        for(int i = 0; i < size; ++i)
+                                    pVal[i] = pCurVal[i] + dt * pDer[i];
+						    }
+
+						} pEulerIntegrate;
+						//
+						struct
+                        {
+                            void operator()(btMultiBody *pBody, const btScalar *pData)
+                            {
+                                btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
+
+                                for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
+                                    pVel[i] = pData[i];
+
+                            }
+                        } pCopyToVelocityVector;
+						//
+                        struct
+						{
+						    void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
+						    {
+						        for(int i = 0; i < size; ++i)
+                                    pDst[i] = pSrc[start + i];
+						    }
+						} pCopy;
+						//
+
+						btScalar h = solverInfo.m_timeStep;
+						#define output &scratch_r[bod->getNumDofs()]
+						//calc qdd0 from: q0 & qd0	
+						bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
+						pCopy(output, scratch_qdd0, 0, numDofs);
+						//calc q1 = q0 + h/2 * qd0
+						pResetQx();
+						bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
+						//calc qd1 = qd0 + h/2 * qdd0
+						pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
+						//
+						//calc qdd1 from: q1 & qd1
+						pCopyToVelocityVector(bod, scratch_qd1);
+						bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
+						pCopy(output, scratch_qdd1, 0, numDofs);
+						//calc q2 = q0 + h/2 * qd1
+						pResetQx();
+						bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
+						//calc qd2 = qd0 + h/2 * qdd1
+						pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
+						//
+						//calc qdd2 from: q2 & qd2
+						pCopyToVelocityVector(bod, scratch_qd2);
+						bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
+						pCopy(output, scratch_qdd2, 0, numDofs);
+						//calc q3 = q0 + h * qd2
+						pResetQx();
+						bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
+						//calc qd3 = qd0 + h * qdd2
+						pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
+						//
+						//calc qdd3 from: q3 & qd3
+						pCopyToVelocityVector(bod, scratch_qd3);
+						bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
+						pCopy(output, scratch_qdd3, 0, numDofs);
+
+						//
+						//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
+						//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)						
+						btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
+						btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
+						for(int i = 0; i < numDofs; ++i)
+						{
+							delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
+							delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);							
+							//delta_q[i] = h*scratch_qd0[i];
+							//delta_qd[i] = h*scratch_qdd0[i];
+						}
+						//
+						pCopyToVelocityVector(bod, scratch_qd0);
+						bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);						
+						//
+						if(!doNotUpdatePos)
+						{
+							btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
+							pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
+
+							for(int i = 0; i < numDofs; ++i)
+								pRealBuf[i] = delta_q[i];
+
+							//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
+							bod->setPosUpdated(true);							
+						}
+
+						//ugly hack which resets the cached data to t0 (needed for constraint solver)
+						{
+							for(int link = 0; link < bod->getNumLinks(); ++link)
+								bod->getLink(link).updateCacheMultiDof();
+							bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m);
+						}
+						
+					}
+				}
+				else//if(bod->isMultiDof())
+				{
+					bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
+				}
+				bod->clearForcesAndTorques();
+			}//if (!isSleeping)
 		}
 	}
 
@@ -503,13 +670,25 @@ void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 			{
 				int nLinks = bod->getNumLinks();
 
-				///base + num links
+				///base + num m_links
 				world_to_local.resize(nLinks+1);
 				local_origin.resize(nLinks+1);
 
-				bod->stepPositions(timeStep);
+				if(bod->isMultiDof())
+				{
+					if(!bod->isPosUpdated())
+						bod->stepPositionsMultiDof(timeStep);
+					else
+					{
+						btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
+						pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
 
-	 
+						bod->stepPositionsMultiDof(1, 0, pRealBuf);
+						bod->setPosUpdated(false);
+					}
+				}
+				else
+					bod->stepPositions(timeStep);			
 
 				world_to_local[0] = bod->getWorldToBaseRot();
 				local_origin[0] = bod->getBasePos();
@@ -517,8 +696,8 @@ void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 				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()};
+				//	float pos[4]={posr.x(),posr.y(),posr.z(),1};
+					btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
 					btTransform tr;
 					tr.setIdentity();
 					tr.setOrigin(posr);
@@ -547,8 +726,8 @@ void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 						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()};
+			//			float pos[4]={posr.x(),posr.y(),posr.z(),1};
+						btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
 						btTransform tr;
 						tr.setIdentity();
 						tr.setOrigin(posr);
@@ -576,3 +755,92 @@ void	btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint*
 {
 	m_multiBodyConstraints.remove(constraint);
 }
+
+void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
+{
+	constraint->debugDraw(getDebugDrawer());
+}
+
+
+void	btMultiBodyDynamicsWorld::debugDrawWorld()
+{
+	BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
+
+	bool drawConstraints = false;
+	if (getDebugDrawer())
+	{
+		int mode = getDebugDrawer()->getDebugMode();
+		if (mode  & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
+		{
+			drawConstraints = true;
+		}
+
+		if (drawConstraints)
+		{
+			BT_PROFILE("btMultiBody debugDrawWorld");
+			
+			btAlignedObjectArray<btQuaternion> world_to_local;
+			btAlignedObjectArray<btVector3> local_origin;
+
+			for (int c=0;c<m_multiBodyConstraints.size();c++)
+			{
+				btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
+				debugDrawMultiBodyConstraint(constraint);
+			}
+
+			for (int b = 0; b<m_multiBodies.size(); b++)
+			{
+				btMultiBody* bod = m_multiBodies[b];
+				int nLinks = bod->getNumLinks();
+
+				///base + num m_links
+				world_to_local.resize(nLinks + 1);
+				local_origin.resize(nLinks + 1);
+
+					
+				world_to_local[0] = bod->getWorldToBaseRot();
+				local_origin[0] = bod->getBasePos();
+
+				
+				{
+					btVector3 posr = local_origin[0];
+					//	float pos[4]={posr.x(),posr.y(),posr.z(),1};
+					btScalar quat[4] = { -world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w() };
+					btTransform tr;
+					tr.setIdentity();
+					tr.setOrigin(posr);
+					tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+
+					getDebugDrawer()->drawTransform(tr, 0.1);
+
+				}
+
+				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++)
+				{
+					int link = m;
+					int index = link + 1;
+
+					btVector3 posr = local_origin[index];
+					//			float pos[4]={posr.x(),posr.y(),posr.z(),1};
+					btScalar quat[4] = { -world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w() };
+					btTransform tr;
+					tr.setIdentity();
+					tr.setOrigin(posr);
+					tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+
+					getDebugDrawer()->drawTransform(tr, 0.1);
+				}
+			}
+		}
+	}
+
+	btDiscreteDynamicsWorld::debugDrawWorld();
+}

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

@@ -38,7 +38,7 @@ protected:
 	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);
@@ -52,5 +52,11 @@ public:
 	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);
 
 	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
+
+	virtual void	integrateTransforms(btScalar timeStep);
+
+	virtual void	debugDrawWorld();
+	
+	virtual void	debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
 };
 #endif //BT_MULTIBODY_DYNAMICS_WORLD_H

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

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

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

@@ -36,7 +36,11 @@ public:
 	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
 		btMultiBodyJacobianData& data,
 		const btContactSolverInfo& infoGlobal);
-	
+
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
 	
 };
 

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

@@ -1,89 +1,118 @@
-/*
-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);
-	}
-
-}
-	
+/*
+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)	
+{
+	int linkDoF = 0;
+
+	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
+
+    unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link);
+
+	// row 0: the lower bound
+	// row 0: the lower bound
+    jacobianA(0)[offset] = 1;
+}
+
+
+btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
+	//:btMultiBodyConstraint(body,0,link,-1,1,true),
+	:btMultiBodyConstraint(body,body,link,link,1,true),
+	m_desiredVelocity(desiredVelocity)	
+{
+	btAssert(linkDoF < body->getLink(link).m_dofCount);
+
+	m_maxAppliedImpulse = maxMotorImpulse;
+	// the data.m_jacobians never change, so may as well
+    // initialize them here
+        
+    // note: we rely on the fact that data.m_jacobians are
+    // always initialized to zero by the Constraint ctor
+
+    unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link);
+
+	// row 0: the lower bound
+	// row 0: the lower bound
+    jacobianA(0)[offset] = 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.
+    
+  
+	const btScalar posError = 0;
+	const btVector3 dummy(0, 0, 0);
+
+	for (int row=0;row<getNumRows();row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		
+		
+		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
+	}
+
+}
+	

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

@@ -1,47 +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.
-*/
-
-///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
-
+/*
+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);
+	btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
+	virtual ~btMultiBodyJointMotor();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+		btMultiBodyJacobianData& data,
+		const btContactSolverInfo& infoGlobal);
+	
+    virtual void setVelocityTarget(btScalar velTarget)
+    {
+        m_desiredVelocity = velTarget;
+    }
+
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
+};
+
+#endif //BT_MULTIBODY_JOINT_MOTOR_H
+

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

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

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

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

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

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

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

@@ -20,6 +20,8 @@ subject to the following restrictions:
 
 #include "btMultiBodyConstraint.h"
 
+//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
+
 class btMultiBodyPoint2Point : public btMultiBodyConstraint
 {
 protected:
@@ -54,6 +56,7 @@ public:
 		m_pivotInB = pivotInB;
 	}
 
+	virtual void debugDraw(class btIDebugDraw* drawer);
 	
 };
 

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

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

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

@@ -821,14 +821,15 @@ 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;
+  int lskip2,i,j;
+//  int lskip3;
   /* 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;
+  //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 */
@@ -1199,9 +1200,9 @@ struct btLCP
 	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 *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
 		btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
-		bool *_state, int *_findex, int *_p, int *_C, btScalar **Arows);
+		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
@@ -1224,9 +1225,9 @@ struct btLCP
 
 
 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 *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
             btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
-            bool *_state, int *_findex, int *_p, int *_C, btScalar **Arows):
+            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),
@@ -1234,8 +1235,8 @@ btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btSc
   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)
+  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);

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

@@ -0,0 +1,371 @@
+/* Copyright (C) 2004-2013 MBSim Development Team
+
+Code was converted for the Bullet Continuous Collision Detection and Physics Library
+
+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.
+*/
+
+//The original version is here
+//https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
+//This file is re-distributed under the ZLib license, with permission of the original author
+//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
+//STL/std::vector replaced by btAlignedObjectArray
+
+
+
+#include "btLemkeAlgorithm.h"
+
+#undef BT_DEBUG_OSTREAM
+#ifdef BT_DEBUG_OSTREAM
+using namespace std;
+#endif //BT_DEBUG_OSTREAM
+
+btScalar btMachEps()
+{
+	static bool calculated=false;
+	static btScalar machEps = btScalar(1.);
+	if (!calculated)
+	{
+		do {
+			machEps /= btScalar(2.0);
+			// If next epsilon yields 1, then break, because current
+			// epsilon is the machine epsilon.
+		}
+		while ((btScalar)(1.0 + (machEps/btScalar(2.0))) != btScalar(1.0));
+//		printf( "\nCalculated Machine epsilon: %G\n", machEps );
+		calculated=true;
+	}
+	return machEps;
+}
+
+btScalar btEpsRoot() {
+	
+	static btScalar epsroot = 0.;
+	static bool alreadyCalculated = false;
+	
+	if (!alreadyCalculated) {
+		epsroot = btSqrt(btMachEps());
+		alreadyCalculated = true;
+	}
+	return epsroot;
+}
+
+	 
+
+  btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
+{
+  
+    
+    steps = 0;
+
+    int dim = m_q.size();
+#ifdef BT_DEBUG_OSTREAM
+    if(DEBUGLEVEL >= 1) {
+      cout << "Dimension = " << dim << endl;
+    }
+#endif //BT_DEBUG_OSTREAM
+
+	btVectorXu solutionVector(2 * dim);
+	solutionVector.setZero();
+	  
+	  //, INIT, 0.);
+
+	btMatrixXu ident(dim, dim);
+	ident.setIdentity();
+#ifdef BT_DEBUG_OSTREAM
+	cout << m_M << std::endl;
+#endif
+
+	btMatrixXu mNeg = m_M.negative();
+	  
+    btMatrixXu A(dim, 2 * dim + 2);
+	//
+	A.setSubMatrix(0, 0, dim - 1, dim - 1,ident);
+	A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1,mNeg);
+	A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f);
+	A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1,m_q);
+
+#ifdef BT_DEBUG_OSTREAM
+	cout << A << std::endl;
+#endif //BT_DEBUG_OSTREAM
+
+
+ //   btVectorXu q_;
+ //   q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1);
+
+    btAlignedObjectArray<int> basis;
+    //At first, all w-values are in the basis
+    for (int i = 0; i < dim; i++)
+      basis.push_back(i);
+
+	int pivotRowIndex = -1;
+	btScalar minValue = 1e30f;
+	bool greaterZero = true;
+	for (int i=0;i<dim;i++)
+	{
+		btScalar v =A(i,2*dim+1);
+		if (v<minValue)
+		{
+			minValue=v;
+			pivotRowIndex = i;
+		}
+		if (v<0)
+			greaterZero = false;
+	}
+	
+
+	
+  //  int pivotRowIndex = q_.minIndex();//minIndex(q_);     // first row is that with lowest q-value
+    int z0Row = pivotRowIndex;           // remember the col of z0 for ending algorithm afterwards
+    int pivotColIndex = 2 * dim;         // first col is that of z0
+
+#ifdef BT_DEBUG_OSTREAM
+    if (DEBUGLEVEL >= 3)
+	{
+    //  cout << "A: " << A << endl;
+      cout << "pivotRowIndex " << pivotRowIndex << endl;
+      cout << "pivotColIndex " << pivotColIndex << endl;
+      cout << "Basis: ";
+      for (int i = 0; i < basis.size(); i++)
+        cout << basis[i] << " ";
+      cout << endl;
+    }
+#endif //BT_DEBUG_OSTREAM
+
+	if (!greaterZero)
+	{
+
+      if (maxloops == 0) {
+		  maxloops = 100;
+//        maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<<dim), but this could exceed UINT_MAX and thus the result would be 0 and therefore the lemke algorithm wouldn't start but probably would find a solution within less then UINT_MAX steps. Therefore this constant is used as a upper border right now...
+      }
+
+      /*start looping*/
+      for(steps = 0; steps < maxloops; steps++) {
+
+        GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
+#ifdef BT_DEBUG_OSTREAM
+        if (DEBUGLEVEL >= 3) {
+        //  cout << "A: " << A << endl;
+          cout << "pivotRowIndex " << pivotRowIndex << endl;
+          cout << "pivotColIndex " << pivotColIndex << endl;
+          cout << "Basis: ";
+          for (int i = 0; i < basis.size(); i++)
+            cout << basis[i] << " ";
+          cout << endl;
+        }
+#endif //BT_DEBUG_OSTREAM
+
+        int pivotColIndexOld = pivotColIndex;
+
+        /*find new column index */
+        if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value
+          pivotColIndex = basis[pivotRowIndex] + dim;
+        else
+          //else do it the other way round and get in the corresponding w-value
+          pivotColIndex = basis[pivotRowIndex] - dim;
+
+        /*the column becomes part of the basis*/
+        basis[pivotRowIndex] = pivotColIndexOld;
+
+        pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
+
+        if(z0Row == pivotRowIndex) { //if z0 leaves the basis the solution is found --> one last elimination step is necessary
+          GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
+          basis[pivotRowIndex] = pivotColIndex; //update basis
+          break;
+      }
+
+      }
+#ifdef BT_DEBUG_OSTREAM
+      if(DEBUGLEVEL >= 1) {
+        cout << "Number of loops: " << steps << endl;
+        cout << "Number of maximal loops: " << maxloops << endl;
+      }
+#endif //BT_DEBUG_OSTREAM
+
+      if(!validBasis(basis)) {
+        info = -1;
+#ifdef BT_DEBUG_OSTREAM
+        if(DEBUGLEVEL >= 1)
+          cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
+#endif //BT_DEBUG_OSTREAM
+
+        return solutionVector;
+      }
+
+    }
+#ifdef BT_DEBUG_OSTREAM
+    if (DEBUGLEVEL >= 2) {
+     // cout << "A: " << A << endl;
+      cout << "pivotRowIndex " << pivotRowIndex << endl;
+      cout << "pivotColIndex " << pivotColIndex << endl;
+    }
+#endif //BT_DEBUG_OSTREAM
+
+    for (int i = 0; i < basis.size(); i++)
+	{
+      solutionVector[basis[i]] = A(i,2*dim+1);//q_[i];
+	}
+
+    info = 0;
+
+    return solutionVector;
+  }
+
+  int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int & pivotColIndex) {
+	  int RowIndex = 0;
+	  int dim = A.rows();
+	  btAlignedObjectArray<btVectorXu> Rows;
+	  for (int row = 0; row < dim; row++) 
+	  {
+
+		  btVectorXu vec(dim + 1);
+		  vec.setZero();//, INIT, 0.)
+		  Rows.push_back(vec);
+		  btScalar a = A(row, pivotColIndex);
+		  if (a > 0) {
+			  Rows[row][0] = A(row, 2 * dim + 1) / a;
+			  Rows[row][1] = A(row, 2 * dim) / a;
+			  for (int j = 2; j < dim + 1; j++)
+				  Rows[row][j] = A(row, j - 1) / a;
+
+#ifdef BT_DEBUG_OSTREAM
+		//		if (DEBUGLEVEL) {
+			//	  cout << "Rows(" << row << ") = " << Rows[row] << endl;
+				// }
+#endif 
+		  }
+	  }
+
+	  for (int i = 0; i < Rows.size(); i++) 
+	  {
+		  if (Rows[i].nrm2() > 0.) {
+
+			  int j = 0;
+			  for (; j < Rows.size(); j++) 
+			  {
+				  if(i != j)
+				  {
+					  if(Rows[j].nrm2() > 0.)
+					  {
+						  btVectorXu test(dim + 1);
+						  for (int ii=0;ii<dim+1;ii++)
+						  {
+							  test[ii] = Rows[j][ii] - Rows[i][ii];
+						  }
+
+						  //=Rows[j] - Rows[i]
+						  if (! LexicographicPositive(test))
+							  break;
+					  }
+				  }
+			  }
+
+			  if (j == Rows.size()) 
+			  {
+				  RowIndex += i;
+				  break;
+			  }
+		  }
+	  }
+
+	  return RowIndex;
+  }
+
+  bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu & v)
+{
+    int i = 0;
+  //  if (DEBUGLEVEL)
+    //  cout << "v " << v << endl;
+
+    while(i < v.size()-1 && fabs(v[i]) < btMachEps())
+      i++;
+    if (v[i] > 0)
+      return true;
+
+    return false;
+  }
+
+void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis) 
+{
+
+	btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex);
+#ifdef BT_DEBUG_OSTREAM
+	cout << A << std::endl;
+#endif
+
+    for (int i = 0; i < A.rows(); i++)
+	{
+      if (i != pivotRowIndex)
+	  {
+        for (int j = 0; j < A.cols(); j++)
+		{
+          if (j != pivotColumnIndex)
+		  {
+			  btScalar v = A(i, j);
+			  v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
+            A.setElem(i, j, v);
+		  }
+		}
+	  }
+	}
+
+#ifdef BT_DEBUG_OSTREAM
+	cout << A << std::endl;
+#endif //BT_DEBUG_OSTREAM
+    for (int i = 0; i < A.cols(); i++) 
+	{
+      A.mulElem(pivotRowIndex, i,-a);
+    }
+#ifdef BT_DEBUG_OSTREAM
+	cout << A << std::endl;
+#endif //#ifdef BT_DEBUG_OSTREAM
+
+    for (int i = 0; i < A.rows(); i++)
+	{
+      if (i != pivotRowIndex)
+	  {
+        A.setElem(i, pivotColumnIndex,0);
+	  }
+	}
+#ifdef BT_DEBUG_OSTREAM
+	cout << A << std::endl;
+#endif //#ifdef BT_DEBUG_OSTREAM
+  }
+
+  bool btLemkeAlgorithm::greaterZero(const btVectorXu & vector)
+{
+    bool isGreater = true;
+    for (int i = 0; i < vector.size(); i++) {
+      if (vector[i] < 0) {
+        isGreater = false;
+        break;
+      }
+    }
+
+    return isGreater;
+  }
+
+  bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis) 
+  {
+    bool isValid = true;
+    for (int i = 0; i < basis.size(); i++) {
+      if (basis[i] >= basis.size() * 2) { //then z0 is in the base
+        isValid = false;
+        break;
+      }
+    }
+
+    return isValid;
+  }
+
+

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

@@ -0,0 +1,108 @@
+/* Copyright (C) 2004-2013 MBSim Development Team
+
+Code was converted for the Bullet Continuous Collision Detection and Physics Library
+
+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.
+*/
+
+//The original version is here
+//https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
+//This file is re-distributed under the ZLib license, with permission of the original author (Kilian Grundl)
+//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
+//STL/std::vector replaced by btAlignedObjectArray
+
+
+
+#ifndef BT_NUMERICS_LEMKE_ALGORITHM_H_
+#define BT_NUMERICS_LEMKE_ALGORITHM_H_
+
+#include "LinearMath/btMatrixX.h"
+
+
+#include <vector> //todo: replace by btAlignedObjectArray
+
+class btLemkeAlgorithm
+{
+public:
+ 
+
+  btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int & DEBUGLEVEL_ = 0) :
+	  DEBUGLEVEL(DEBUGLEVEL_)
+  {
+	setSystem(M_, q_);
+  }
+
+  /* GETTER / SETTER */
+  /**
+   * \brief return info of solution process
+   */
+  int getInfo() {
+	return info;
+  }
+
+  /**
+   * \brief get the number of steps until the solution was found
+   */
+  int getSteps(void) {
+	return steps;
+  }
+
+
+
+  /**
+   * \brief set system with Matrix M and vector q
+   */
+  void setSystem(const btMatrixXu & M_, const btVectorXu & q_)
+	{
+		m_M = M_;
+		m_q = q_;
+  }
+  /***************************************************/
+
+  /**
+   * \brief solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd)
+   */
+  btVectorXu solve(unsigned int maxloops = 0);
+
+  virtual ~btLemkeAlgorithm() {
+  }
+
+protected:
+  int findLexicographicMinimum(const btMatrixXu &A, const int & pivotColIndex);
+  bool LexicographicPositive(const btVectorXu & v);
+  void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis);
+  bool greaterZero(const btVectorXu & vector);
+  bool validBasis(const btAlignedObjectArray<int>& basis);
+
+  btMatrixXu m_M;
+  btVectorXu m_q;
+
+  /**
+   * \brief number of steps until the Lemke algorithm found a solution
+   */
+  unsigned int steps;
+
+  /**
+   * \brief define level of debug output
+   */
+  int DEBUGLEVEL;
+
+  /**
+   * \brief did the algorithm find a solution
+   *
+   * -1 : not successful
+   *  0 : successful
+   */
+  int info;
+};
+
+
+#endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */

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

@@ -0,0 +1,350 @@
+/*
+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_LEMKE_SOLVER_H
+#define BT_LEMKE_SOLVER_H
+
+
+#include "btMLCPSolverInterface.h"
+#include "btLemkeAlgorithm.h"
+
+
+
+
+///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
+///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time.
+///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team
+class btLemkeSolver : public btMLCPSolverInterface
+{
+protected:
+	
+public:
+
+	btScalar	m_maxValue;
+	int			m_debugLevel;
+	int			m_maxLoops;
+	bool		m_useLoHighBounds;
+	
+	
+
+	btLemkeSolver()
+		:m_maxValue(100000),
+		m_debugLevel(0),
+		m_maxLoops(1000),
+		m_useLoHighBounds(true)
+	{
+	}
+	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)
+	{
+		
+		if (m_useLoHighBounds)
+		{
+
+		BT_PROFILE("btLemkeSolver::solveMLCP");
+		int n = A.rows();
+		if (0==n)
+			return true;
+		
+		bool fail = false;
+
+		btVectorXu solution(n);
+		btVectorXu q1;
+		q1.resize(n);
+		for (int row=0;row<n;row++)
+		{
+			q1[row] = -b[row];
+		}
+
+	//		cout << "A" << endl;
+	//		cout << A << endl;
+
+			/////////////////////////////////////
+
+			//slow matrix inversion, replace with LU decomposition
+			btMatrixXu A1;
+			btMatrixXu B(n,n);
+			{
+				BT_PROFILE("inverse(slow)");
+				A1.resize(A.rows(),A.cols());
+				for (int row=0;row<A.rows();row++)
+				{
+					for (int col=0;col<A.cols();col++)
+					{
+						A1.setElem(row,col,A(row,col));
+					}
+				}
+
+				btMatrixXu matrix;
+				matrix.resize(n,2*n);
+				for (int row=0;row<n;row++)
+				{
+					for (int col=0;col<n;col++)
+					{
+						matrix.setElem(row,col,A1(row,col));
+					}
+				}
+
+
+				btScalar ratio,a;
+				int i,j,k;
+				for(i = 0; i < n; i++){
+				for(j = n; j < 2*n; j++){
+					if(i==(j-n))
+						matrix.setElem(i,j,1.0);
+					else
+						matrix.setElem(i,j,0.0);
+				}
+			}
+			for(i = 0; i < n; i++){
+				for(j = 0; j < n; j++){
+					if(i!=j)
+					{
+						btScalar v = matrix(i,i);
+						if (btFuzzyZero(v))
+						{
+							a = 0.000001f;
+						}
+						ratio = matrix(j,i)/matrix(i,i);
+						for(k = 0; k < 2*n; k++){
+							matrix.addElem(j,k,- ratio * matrix(i,k));
+						}
+					}
+				}
+			}
+			for(i = 0; i < n; i++){
+				a = matrix(i,i);
+				if (btFuzzyZero(a))
+				{
+					a = 0.000001f;
+				}
+				btScalar invA = 1.f/a;
+				for(j = 0; j < 2*n; j++){
+					matrix.mulElem(i,j,invA);
+				}
+			}
+
+	
+
+	
+
+			for (int row=0;row<n;row++)
+				{
+					for (int col=0;col<n;col++)
+					{
+						B.setElem(row,col,matrix(row,n+col));
+					}
+				}
+			}
+
+		btMatrixXu b1(n,1);
+
+		btMatrixXu M(n*2,n*2);
+		for (int row=0;row<n;row++)
+		{
+			b1.setElem(row,0,-b[row]);
+			for (int col=0;col<n;col++)
+			{
+				btScalar v =B(row,col);
+				M.setElem(row,col,v);
+				M.setElem(n+row,n+col,v);
+				M.setElem(n+row,col,-v);
+				M.setElem(row,n+col,-v);
+
+			}
+		}
+
+		btMatrixXu Bb1 = B*b1;
+//		q = [ (-B*b1 - lo)'   (hi + B*b1)' ]'
+
+		btVectorXu qq;
+		qq.resize(n*2);
+		for (int row=0;row<n;row++)
+		{
+			qq[row] = -Bb1(row,0)-lo[row];
+			qq[n+row] = Bb1(row,0)+hi[row];
+		}
+
+		btVectorXu z1;
+
+		btMatrixXu y1;
+		y1.resize(n,1);
+		btLemkeAlgorithm lemke(M,qq,m_debugLevel);
+		{
+			BT_PROFILE("lemke.solve");
+			lemke.setSystem(M,qq);
+			z1  = lemke.solve(m_maxLoops);
+		}
+		for (int row=0;row<n;row++)
+		{
+			y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]);
+		}
+		btMatrixXu y1_b1(n,1);
+		for (int i=0;i<n;i++)
+		{
+			y1_b1.setElem(i,0,y1(i,0)-b1(i,0));
+		}
+
+		btMatrixXu x1;
+
+		x1 = B*(y1_b1);
+			
+		for (int row=0;row<n;row++)
+		{
+			solution[row] = x1(row,0);//n];
+		}
+
+		int errorIndexMax = -1;
+		int errorIndexMin = -1;
+		float errorValueMax = -1e30;
+		float errorValueMin = 1e30;
+		
+		for (int i=0;i<n;i++)
+		{
+			x[i] = solution[i];
+			volatile btScalar check = x[i];
+			if (x[i] != check)
+			{
+				//printf("Lemke result is #NAN\n");
+				x.setZero();
+				return false;
+			}
+			
+			//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver 
+			//we need to figure out why it happens, and fix it, or detect it properly)
+			if (x[i]>m_maxValue)
+			{
+				if (x[i]> errorValueMax)
+				{
+					fail = true;
+					errorIndexMax = i;
+					errorValueMax = x[i];
+				}
+				////printf("x[i] = %f,",x[i]);
+			}
+			if (x[i]<-m_maxValue)
+			{
+				if (x[i]<errorValueMin)
+				{
+					errorIndexMin = i;
+					errorValueMin = x[i];
+					fail = true;
+					//printf("x[i] = %f,",x[i]);
+				}
+			}
+		}
+		if (fail)
+		{
+			int m_errorCountTimes = 0;
+			if (errorIndexMin<0)
+				errorValueMin = 0.f;
+			if (errorIndexMax<0)
+				errorValueMax = 0.f;
+			m_errorCountTimes++;
+		//	printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
+			for (int i=0;i<n;i++)
+			{
+				x[i]=0.f;
+			}
+		}
+		return !fail;
+	} else
+		
+	{
+			int dimension = A.rows();
+		if (0==dimension)
+			return true;
+		
+//		printf("================ solving using Lemke/Newton/Fixpoint\n");
+
+		btVectorXu q;
+		q.resize(dimension);
+		for (int row=0;row<dimension;row++)
+		{
+			q[row] = -b[row];
+		}
+		
+		btLemkeAlgorithm lemke(A,q,m_debugLevel);
+		
+		
+		lemke.setSystem(A,q);
+		
+		btVectorXu solution = lemke.solve(m_maxLoops);
+		
+		//check solution
+		
+		bool fail = false;
+		int errorIndexMax = -1;
+		int errorIndexMin = -1;
+		float errorValueMax = -1e30;
+		float errorValueMin = 1e30;
+		
+		for (int i=0;i<dimension;i++)
+		{
+			x[i] = solution[i+dimension];
+			volatile btScalar check = x[i];
+			if (x[i] != check)
+			{
+				x.setZero();
+				return false;
+			}
+			
+			//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver 
+			//we need to figure out why it happens, and fix it, or detect it properly)
+			if (x[i]>m_maxValue)
+			{
+				if (x[i]> errorValueMax)
+				{
+					fail = true;
+					errorIndexMax = i;
+					errorValueMax = x[i];
+				}
+				////printf("x[i] = %f,",x[i]);
+			}
+			if (x[i]<-m_maxValue)
+			{
+				if (x[i]<errorValueMin)
+				{
+					errorIndexMin = i;
+					errorValueMin = x[i];
+					fail = true;
+					//printf("x[i] = %f,",x[i]);
+				}
+			}
+		}
+		if (fail)
+		{
+			static int errorCountTimes = 0;
+			if (errorIndexMin<0)
+				errorValueMin = 0.f;
+			if (errorIndexMax<0)
+				errorValueMax = 0.f;
+			printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
+			for (int i=0;i<dimension;i++)
+			{
+				x[i]=0.f;
+			}
+		}
+
+
+		return !fail;
+	}
+	return true;
+
+	}
+
+};
+
+#endif //BT_LEMKE_SOLVER_H

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

@@ -19,9 +19,11 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "btSolveProjectedGaussSeidel.h"
 
+
 btMLCPSolver::btMLCPSolver(	 btMLCPSolverInterface* solver)
 :m_solver(solver),
-m_fallback(0)
+m_fallback(0),
+m_cfm(0.000001)//0.0000001
 {
 }
 
@@ -42,8 +44,8 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
 		int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2;
 
 
-		int numBodies = m_tmpSolverBodyPool.size();
-		m_allConstraintArray.resize(0);
+	//	int numBodies = m_tmpSolverBodyPool.size();
+		m_allConstraintPtrArray.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());
@@ -51,7 +53,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
 		int dindex = 0;
 		for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++)
 		{
-			m_allConstraintArray.push_back(m_tmpSolverNonContactConstraintPool[i]);
+			m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
 			m_limitDependencies[dindex++] = -1;
 		}
  
@@ -63,14 +65,14 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
 		{
 			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
 			{
-				m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
 				m_limitDependencies[dindex++] = -1;
-				m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
 				int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact));
 				m_limitDependencies[dindex++] = findex +firstContactConstraintOffset;
 				if (numFrictionPerContact==2)
 				{
-					m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
+					m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
 					m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
 				}
 			}
@@ -78,19 +80,19 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
 		{
 			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
 			{
-				m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
+				m_allConstraintPtrArray.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_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
 				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
 			}
 			
 		}
 
 
-		if (!m_allConstraintArray.size())
+		if (!m_allConstraintPtrArray.size())
 		{
 			m_A.resize(0,0);
 			m_b.resize(0);
@@ -154,26 +156,30 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 {
 	int numContactRows = interleaveContactAndFriction ? 3 : 1;
 
-	int numConstraintRows = m_allConstraintArray.size();
+	int numConstraintRows = m_allConstraintPtrArray.size();
 	int n = numConstraintRows;
 	{
 		BT_PROFILE("init b (rhs)");
 		m_b.resize(numConstraintRows);
 		m_bSplit.resize(numConstraintRows);
-		//m_b.setZero();
+		m_b.setZero();
+		m_bSplit.setZero();
 		for (int i=0;i<numConstraintRows ;i++)
 		{
-			if (m_allConstraintArray[i].m_jacDiagABInv)
+			btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
+			if (!btFuzzyZero(jacDiag))
 			{
-				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 rhs = m_allConstraintPtrArray[i]->m_rhs;
+				btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
+				m_b[i]=rhs/jacDiag;
+				m_bSplit[i] = rhsPenetration/jacDiag;
 			}
 
 		}
 	}
 
-	btScalar* w = 0;
-	int nub = 0;
+//	btScalar* w = 0;
+//	int nub = 0;
 
 	m_lo.resize(numConstraintRows);
 	m_hi.resize(numConstraintRows);
@@ -189,14 +195,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 				m_hi[i] = BT_INFINITY;
 			} else
 			{
-				m_lo[i] = m_allConstraintArray[i].m_lowerLimit;
-				m_hi[i] = m_allConstraintArray[i].m_upperLimit;
+				m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
+				m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
 			}
 		}
 	}
 
 	//
-	int m=m_allConstraintArray.size();
+	int m=m_allConstraintPtrArray.size();
 
 	int numBodies = m_tmpSolverBodyPool.size();
 	btAlignedObjectArray<int> bodyJointNodeArray;
@@ -207,7 +213,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 	btAlignedObjectArray<btJointNode> jointNodeArray;
 	{
 		BT_PROFILE("jointNodeArray.reserve");
-		jointNodeArray.reserve(2*m_allConstraintArray.size());
+		jointNodeArray.reserve(2*m_allConstraintPtrArray.size());
 	}
 
 	static btMatrixXu J3;
@@ -229,7 +235,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 	{
 		BT_PROFILE("ofs resize");
 		ofs.resize(0);
-		ofs.resizeNoInitialize(m_allConstraintArray.size());
+		ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
 	}				
 	{
 		BT_PROFILE("Compute J and JinvM");
@@ -237,11 +243,11 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 
 		int numRows = 0;
 
-		for (int i=0;i<m_allConstraintArray.size();i+=numRows,c++)
+		for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++)
 		{
 			ofs[c] = rowOffset;
-			int sbA = m_allConstraintArray[i].m_solverBodyIdA;
-			int sbB = m_allConstraintArray[i].m_solverBodyIdB;
+			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
 			btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
 			btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 
@@ -262,13 +268,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 				}
 				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();
+					btVector3 normalInvMass =				m_allConstraintPtrArray[i+row]->m_contactNormal1 *		orgBodyA->getInvMass();
+					btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal *	orgBodyA->getInvInertiaTensorWorld();
 
 					for (int r=0;r<3;r++)
 					{
-						J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal1[r]);
-						J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos1CrossNormal[r]);
+						J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]);
+						J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]);
 						JinvM3.setElem(cur,r,normalInvMass[r]);
 						JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]);
 					}
@@ -299,13 +305,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 
 				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();
+					btVector3 normalInvMassB = m_allConstraintPtrArray[i+row]->m_contactNormal2*orgBodyB->getInvMass();
+					btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
 
 					for (int r=0;r<3;r++)
 					{
-						J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal2[r]);
-						J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos2CrossNormal[r]);
+						J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]);
+						J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]);
 						JinvM3.setElem(cur,r,normalInvMassB[r]);
 						JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]);
 					}
@@ -343,13 +349,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 	{
 		int numRows = 0;
 		BT_PROFILE("Compute A");
-		for (int i=0;i<m_allConstraintArray.size();i+= numRows,c++)
+		for (int i=0;i<m_allConstraintPtrArray.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;
+			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
+		//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+		//	btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 
 			numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
 					
@@ -365,7 +371,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 					{
 								 
 						int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
-						size_t ofsother = (m_allConstraintArray[cr0].m_solverBodyIdB == sbA) ? 8*numRowsOther  : 0;
+						size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8*numRowsOther  : 0;
 						//printf("%d joint i %d and j0: %d: ",count++,i,j0);
 						m_A.multiplyAdd2_p8r ( JinvMrow, 
 						Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther,  row__,ofs[j0]);
@@ -384,7 +390,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 					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;
+						size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8*numRowsOther  : 0;
 						m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, 
 						Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]);
 					}
@@ -398,15 +404,15 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 			// compute diagonal blocks of m_A
 
 			int  row__ = 0;
-			int numJointRows = m_allConstraintArray.size();
+			int numJointRows = m_allConstraintPtrArray.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;
+				//int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
+				int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
+			//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
 				btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 
 
@@ -425,14 +431,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 		}
 	}
 
-	///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);
+			m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
 		}
 	}
 				   
@@ -449,9 +453,9 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 
 		if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
 		{
-			for (int i=0;i<m_allConstraintArray.size();i++)
+			for (int i=0;i<m_allConstraintPtrArray.size();i++)
 			{
-				const btSolverConstraint& c = m_allConstraintArray[i];
+				const btSolverConstraint& c = *m_allConstraintPtrArray[i];
 				m_x[i]=c.m_appliedImpulse;
 				m_xSplit[i] = c.m_appliedPushImpulse;
 			}
@@ -467,19 +471,22 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 {
 	int numBodies = this->m_tmpSolverBodyPool.size();
-	int numConstraintRows = m_allConstraintArray.size();
+	int numConstraintRows = m_allConstraintPtrArray.size();
 
 	m_b.resize(numConstraintRows);
 	if (infoGlobal.m_splitImpulse)
 		m_bSplit.resize(numConstraintRows);
  
+	m_bSplit.setZero();
+	m_b.setZero();
+
 	for (int i=0;i<numConstraintRows ;i++)
 	{
-		if (m_allConstraintArray[i].m_jacDiagABInv)
+		if (m_allConstraintPtrArray[i]->m_jacDiagABInv)
 		{
-			m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
+			m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv;
 			if (infoGlobal.m_splitImpulse)
-				m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
+				m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv;
 		}
 	}
  
@@ -510,28 +517,28 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 	for (int i=0;i<numConstraintRows;i++)
 	{
 
-		m_lo[i] = m_allConstraintArray[i].m_lowerLimit;
-		m_hi[i] = m_allConstraintArray[i].m_upperLimit;
+		m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
+		m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
  
-		int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA;
-		int bodyIndex1 = m_allConstraintArray[i].m_solverBodyIdB;
+		int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+		int bodyIndex1 = m_allConstraintPtrArray[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]);
+			setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]);
+			setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]);
+			setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]);
+			setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]);
+			setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]);
+			setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[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]);
+			setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]);
+			setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]);
+			setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]);
+			setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]);
+			setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]);
+			setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]);
 		}
 	}
  
@@ -554,12 +561,10 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 
 	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_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
 		}
 	}
 
@@ -568,9 +573,9 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 		m_xSplit.resize(numConstraintRows);
 //	m_x.setZero();
 
-	for (int i=0;i<m_allConstraintArray.size();i++)
+	for (int i=0;i<m_allConstraintPtrArray.size();i++)
 	{
-		const btSolverConstraint& c = m_allConstraintArray[i];
+		const btSolverConstraint& c = *m_allConstraintPtrArray[i];
 		m_x[i]=c.m_appliedImpulse;
 		if (infoGlobal.m_splitImpulse)
 			m_xSplit[i] = c.m_appliedPushImpulse;
@@ -592,35 +597,44 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
 	if (result)
 	{
 		BT_PROFILE("process MLCP results");
-		for (int i=0;i<m_allConstraintArray.size();i++)
+		for (int i=0;i<m_allConstraintPtrArray.size();i++)
 		{
 			{
-				btSolverConstraint& c = m_allConstraintArray[i];
+				btSolverConstraint& c = *m_allConstraintPtrArray[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;
+				//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]);
+				{
+					btScalar deltaImpulse = m_x[i]-c.m_appliedImpulse;
+					c.m_appliedImpulse = m_x[i];
+					solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+					solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+				}
+
 				if (infoGlobal.m_splitImpulse)
 				{
-					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]);
+					btScalar deltaImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
+					solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+					solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
 					c.m_appliedPushImpulse = m_xSplit[i];
 				}
-				c.m_appliedImpulse = m_x[i];
+				
 			}
 		}
 	}
 	else
 	{
+	//	printf("m_fallback = %d\n",m_fallback);
 		m_fallback++;
 		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
 	}
 
 	return 0.f;
-}
+}
+
+

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

@@ -39,12 +39,15 @@ protected:
 	btVectorXu m_xSplit2;
 
 	btAlignedObjectArray<int> m_limitDependencies;
-	btConstraintArray m_allConstraintArray;
+	btAlignedObjectArray<btSolverConstraint*>	m_allConstraintPtrArray;
 	btMLCPSolverInterface* m_solver;
 	int m_fallback;
+	btScalar m_cfm;
 
 	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);
 
@@ -70,6 +73,15 @@ public:
 		m_fallback = num;
 	}
 
+	btScalar	getCfm() const
+	{
+		return m_cfm;
+	}
+	void setCfm(btScalar cfm)
+	{
+		m_cfm = cfm;
+	}
+
 	virtual btConstraintSolverType	getSolverType() const
 	{
 		return BT_MLCP_SOLVER;

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

@@ -20,11 +20,17 @@ subject to the following restrictions:
 
 #include "btMLCPSolverInterface.h"
 
+///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix)
 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)
 	{
+		if (!A.rows())
+			return true;
+		//the A matrix is sparse, so compute the non-zero elements
+		A.rowComputeNonZeroElements();
+
 		//A is a m-n matrix, m rows, n columns
 		btAssert(A.rows() == b.rows());
 
@@ -56,7 +62,7 @@ public:
 				}
 
 				float aDiag = A(i,i);
-				x [i] = (b [i] - delta) / A(i,i);
+				x [i] = (b [i] - delta) / aDiag;
 				float s = 1.f;
 
 				if (limitDependency[i]>=0)

Source/ThirdParty/Bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp

@@ -296,8 +296,9 @@ void btRaycastVehicle::updateVehicle( btScalar step )
 	int i=0;
 	for (i=0;i<m_wheelInfo.size();i++)
 	{
-		btScalar depth; 
-		depth = rayCast( m_wheelInfo[i]);
+		//btScalar depth; 
+		//depth = 
+		rayCast( m_wheelInfo[i]);
 	}
 
 	updateSuspension(step);

Source/ThirdParty/Bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h

@@ -3027,21 +3027,23 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti)
 	{
 		const RContact&		c = psb->m_rcontacts[i];
 		const sCti&			cti = c.m_cti;	
-		btRigidBody* tmpRigid = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
-
-		const btVector3		va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0);
-		const btVector3		vb = c.m_node->m_x-c.m_node->m_q;	
-		const btVector3		vr = vb-va;
-		const btScalar		dn = btDot(vr, cti.m_normal);		
-		if(dn<=SIMD_EPSILON)
+		if (cti.m_colObj->hasContactResponse()) 
 		{
-			const btScalar		dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg );
-			const btVector3		fv = vr - (cti.m_normal * dn);
-			// c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
-			const btVector3		impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst );
-			c.m_node->m_x -= impulse * c.m_c2;
-			if (tmpRigid)
-				tmpRigid->applyImpulse(impulse,c.m_c1);
+			btRigidBody* tmpRigid = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+			const btVector3		va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0);
+			const btVector3		vb = c.m_node->m_x-c.m_node->m_q;	
+			const btVector3		vr = vb-va;
+			const btScalar		dn = btDot(vr, cti.m_normal);		
+			if(dn<=SIMD_EPSILON)
+			{
+				const btScalar		dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg );
+				const btVector3		fv = vr - (cti.m_normal * dn);
+				// c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
+				const btVector3		impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst );
+				c.m_node->m_x -= impulse * c.m_c2;
+				if (tmpRigid)
+					tmpRigid->applyImpulse(impulse,c.m_c1);
+			}
 		}
 	}
 }
@@ -3603,8 +3605,8 @@ const char*	btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ
 			m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]);
 			m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]);
 			memPtr->m_cfm = m_joints[i]->m_cfm;
-			memPtr->m_erp = m_joints[i]->m_erp;
-			memPtr->m_split = m_joints[i]->m_split;
+			memPtr->m_erp = float(m_joints[i]->m_erp);
+			memPtr->m_split = float(m_joints[i]->m_split);
 			memPtr->m_delete = m_joints[i]->m_delete;
 			
 			for (int j=0;j<4;j++)

Source/ThirdParty/Bullet/src/BulletSoftBody/btSoftBody.h

@@ -171,6 +171,7 @@ public:
 	/* ImplicitFn	*/ 
 	struct	ImplicitFn
 	{
+		virtual ~ImplicitFn() {}
 		virtual btScalar	Eval(const btVector3& x)=0;
 	};
 
@@ -528,6 +529,7 @@ public:
 	{
 		struct IControl
 		{
+			virtual ~IControl() {}
 			virtual void			Prepare(AJoint*)				{}
 			virtual btScalar		Speed(AJoint*,btScalar current) { return(current); }
 			static IControl*		Default()						{ static IControl def;return(&def); }

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

@@ -480,6 +480,168 @@ void			btSoftBodyHelpers::DrawClusterTree(	btSoftBody* psb,
 	drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
 }
 
+
+//The btSoftBody object from the BulletSDK includes an array of Nodes and Links. These links appear
+// to be first set up to connect a node to between 5 and 6 of its neighbors [480 links], 
+//and then to the rest of the nodes after the execution of the Floyd-Warshall graph algorithm 
+//[another 930 links]. 
+//The way the links are stored by default, we have a number of cases where adjacent links share a node in common
+// - this leads to the creation of a data dependency through memory. 
+//The PSolve_Links() function reads and writes nodes as it iterates over each link. 
+//So, we now have the possibility of a data dependency between iteration X 
+//that processes link L with iteration X+1 that processes link L+1 
+//because L and L+1 have one node in common, and iteration X updates the positions of that node, 
+//and iteration X+1 reads in the position of that shared node.
+//
+//Such a memory dependency limits the ability of a modern CPU to speculate beyond 
+//a certain point because it has to respect a possible dependency 
+//- this prevents the CPU from making full use of its out-of-order resources. 
+//If we re-order the links such that we minimize the cases where a link L and L+1 share a common node, 
+//we create a temporal gap between when the node position is written, 
+//and when it is subsequently read. This in turn allows the CPU to continue execution without 
+//risking a dependency violation. Such a reordering would result in significant speedups on 
+//modern CPUs with lots of execution resources. 
+//In our testing, we see it have a tremendous impact not only on the A7, 
+//but also on all x86 cores that ship with modern Macs. 
+//The attached source file includes a single function (ReoptimizeLinkOrder) which can be called on a 
+//btSoftBody object in the solveConstraints() function before the actual solver is invoked, 
+//or right after generateBendingConstraints() once we have all 1410 links.
+
+
+//===================================================================
+//
+//
+// This function takes in a list of interdependent Links and tries 
+// to maximize the distance between calculation
+// of dependent links.  This increases the amount of parallelism that can
+// be exploited by out-of-order instruction processors with large but
+// (inevitably) finite instruction windows.
+//
+//===================================================================
+
+// A small structure to track lists of dependent link calculations
+class LinkDeps_t {
+	public:
+	int value;			// A link calculation that is dependent on this one
+		// Positive values = "input A" while negative values = "input B"
+	LinkDeps_t *next;	// Next dependence in the list
+};
+typedef LinkDeps_t *LinkDepsPtr_t;
+
+// Dependency list constants
+#define REOP_NOT_DEPENDENT	-1
+#define REOP_NODE_COMPLETE	-2	// Must be less than REOP_NOT_DEPENDENT
+
+
+void btSoftBodyHelpers::ReoptimizeLinkOrder(btSoftBody *psb /* This can be replaced by a btSoftBody pointer */)
+{
+	int i, nLinks=psb->m_links.size(), nNodes=psb->m_nodes.size();
+	btSoftBody::Link *lr;
+	int ar, br;
+	btSoftBody::Node *node0 = &(psb->m_nodes[0]);
+	btSoftBody::Node *node1 = &(psb->m_nodes[1]);
+	LinkDepsPtr_t linkDep;
+	int readyListHead, readyListTail, linkNum, linkDepFrees, depLink;
+	
+	// Allocate temporary buffers
+	int *nodeWrittenAt = new int[nNodes+1];	// What link calculation produced this node's current values?
+	int *linkDepA = new int[nLinks];			// Link calculation input is dependent upon prior calculation #N
+	int *linkDepB = new int[nLinks];
+	int *readyList = new int[nLinks];		// List of ready-to-process link calculations (# of links, maximum)
+	LinkDeps_t *linkDepFreeList = new LinkDeps_t[2*nLinks];		// Dependent-on-me list elements (2x# of links, maximum)
+	LinkDepsPtr_t *linkDepListStarts = new LinkDepsPtr_t[nLinks];	// Start nodes of dependent-on-me lists, one for each link
+		
+	// Copy the original, unsorted links to a side buffer
+	btSoftBody::Link *linkBuffer = new btSoftBody::Link[nLinks];
+	memcpy(linkBuffer, &(psb->m_links[0]), sizeof(btSoftBody::Link)*nLinks);
+
+	// Clear out the node setup and ready list
+	for (i=0; i < nNodes+1; i++) {
+		nodeWrittenAt[i] = REOP_NOT_DEPENDENT;
+	}
+	for (i=0; i < nLinks; i++) {
+		linkDepListStarts[i] = NULL;
+	}
+	readyListHead = readyListTail = linkDepFrees = 0;
+
+	// Initial link analysis to set up data structures
+	for (i=0; i < nLinks; i++) {
+	
+		// Note which prior link calculations we are dependent upon & build up dependence lists
+		lr = &(psb->m_links[i]);
+		ar = (lr->m_n[0] - node0)/(node1 - node0);
+		br = (lr->m_n[1] - node0)/(node1 - node0);
+		if (nodeWrittenAt[ar] > REOP_NOT_DEPENDENT) {
+			linkDepA[i] = nodeWrittenAt[ar];
+			linkDep = &linkDepFreeList[linkDepFrees++];
+			linkDep->value = i;
+			linkDep->next = linkDepListStarts[nodeWrittenAt[ar]];
+			linkDepListStarts[nodeWrittenAt[ar]] = linkDep;
+		} else {
+			linkDepA[i] = REOP_NOT_DEPENDENT;
+		}
+		if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT) {
+			linkDepB[i] = nodeWrittenAt[br];
+			linkDep = &linkDepFreeList[linkDepFrees++];
+			linkDep->value = -(i+1);
+			linkDep->next = linkDepListStarts[nodeWrittenAt[br]];
+			linkDepListStarts[nodeWrittenAt[br]] = linkDep;
+		} else {
+			linkDepB[i] = REOP_NOT_DEPENDENT;
+		}
+		
+		// Add this link to the initial ready list, if it is not dependent on any other links
+		if ((linkDepA[i] == REOP_NOT_DEPENDENT) && (linkDepB[i] == REOP_NOT_DEPENDENT)) {
+			readyList[readyListTail++] = i;
+			linkDepA[i] = linkDepB[i] = REOP_NODE_COMPLETE;	// Probably not needed now
+		}
+		
+		// Update the nodes to mark which ones are calculated by this link
+		nodeWrittenAt[ar] = nodeWrittenAt[br] = i;
+	}
+	
+	// Process the ready list and create the sorted list of links
+	// -- By treating the ready list as a queue, we maximize the distance between any
+	//    inter-dependent node calculations
+	// -- All other (non-related) nodes in the ready list will automatically be inserted
+	//    in between each set of inter-dependent link calculations by this loop
+	i = 0;
+	while (readyListHead != readyListTail) {
+		// Use ready list to select the next link to process
+		linkNum = readyList[readyListHead++];
+		// Copy the next-to-calculate link back into the original link array
+		psb->m_links[i++] = linkBuffer[linkNum];
+
+		// Free up any link inputs that are dependent on this one
+		linkDep = linkDepListStarts[linkNum];
+		while (linkDep) {
+			depLink = linkDep->value;
+			if (depLink >= 0) {
+				linkDepA[depLink] = REOP_NOT_DEPENDENT;
+			} else {
+				depLink = -depLink - 1;
+				linkDepB[depLink] = REOP_NOT_DEPENDENT;
+			}
+			// Add this dependent link calculation to the ready list if *both* inputs are clear
+			if ((linkDepA[depLink] == REOP_NOT_DEPENDENT) && (linkDepB[depLink] == REOP_NOT_DEPENDENT)) {
+				readyList[readyListTail++] = depLink;
+				linkDepA[depLink] = linkDepB[depLink] = REOP_NODE_COMPLETE;	// Probably not needed now
+			}
+			linkDep = linkDep->next;
+		}
+	}
+
+	// Delete the temporary buffers
+	delete [] nodeWrittenAt;
+	delete [] linkDepA;
+	delete [] linkDepB;
+	delete [] readyList;
+	delete [] linkDepFreeList;
+	delete [] linkDepListStarts;
+	delete [] linkBuffer;
+}
+
+
 //
 void			btSoftBodyHelpers::DrawFrame(		btSoftBody* psb,
 											 btIDebugDraw* idraw)

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

@@ -137,7 +137,12 @@ struct	btSoftBodyHelpers
 													bool bfacelinks,
 													bool btetralinks,
 													bool bfacesfromtetras);
-	
+
+	/// Sort the list of links to move link calculations that are dependent upon earlier
+	/// ones as far as possible away from the calculation of those values
+	/// This tends to make adjacent loop iterations not dependent upon one another,
+	/// so out-of-order processors can execute instructions from multiple iterations at once
+	static void ReoptimizeLinkOrder(btSoftBody *psb );
 };
 
 #endif //BT_SOFT_BODY_HELPERS_H

Source/ThirdParty/Bullet/src/BulletSoftBody/btSoftBodyInternals.h

@@ -76,7 +76,7 @@ void	btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 	btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep );
 	{
 		BT_PROFILE("predictUnconstraintMotionSoftBody");
-		m_softBodySolver->predictMotion( timeStep );
+		m_softBodySolver->predictMotion( float(timeStep) );
 	}
 }
 

Source/ThirdParty/Bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.h

@@ -0,0 +1,92 @@
+
+#ifndef BT_CPU_UTILITY_H
+#define BT_CPU_UTILITY_H
+
+#include "LinearMath/btScalar.h"
+
+#include <string.h>//memset
+#ifdef  USE_SIMD
+#include <emmintrin.h>
+#ifdef BT_ALLOW_SSE4
+#include <intrin.h>
+#endif //BT_ALLOW_SSE4
+#endif //USE_SIMD
+
+#if defined BT_USE_NEON
+#define ARM_NEON_GCC_COMPATIBILITY  1
+#include <arm_neon.h>
+#include <sys/types.h>
+#include <sys/sysctl.h> //for sysctlbyname
+#endif //BT_USE_NEON
+
+///Rudimentary btCpuFeatureUtility for CPU features: only report the features that Bullet actually uses (SSE4/FMA3, NEON_HPFP)
+///We assume SSE2 in case BT_USE_SSE2 is defined in LinearMath/btScalar.h
+class btCpuFeatureUtility
+{
+public:
+	enum btCpuFeature
+	{
+		CPU_FEATURE_FMA3=1,
+		CPU_FEATURE_SSE4_1=2,
+		CPU_FEATURE_NEON_HPFP=4
+	};
+
+	static int getCpuFeatures()
+	{
+
+		static int capabilities = 0;
+		static bool testedCapabilities = false;
+		if (0 != testedCapabilities)
+		{
+			return capabilities;
+		}
+
+#ifdef BT_USE_NEON
+		{
+			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 |= CPU_FEATURE_NEON_HPFP;
+		}
+#endif //BT_USE_NEON
+
+#ifdef  BT_ALLOW_SSE4
+		{
+			int					cpuInfo[4];
+			memset(cpuInfo, 0, sizeof(cpuInfo));
+			unsigned long long	sseExt = 0;
+			__cpuid(cpuInfo, 1);
+			
+			bool osUsesXSAVE_XRSTORE = cpuInfo[2] & (1 << 27) || false;
+			bool cpuAVXSuport = cpuInfo[2] & (1 << 28) || false;
+
+			if (osUsesXSAVE_XRSTORE && cpuAVXSuport)
+			{
+				sseExt = _xgetbv(0);
+			}
+			const int OSXSAVEFlag = (1UL << 27);
+			const int AVXFlag = ((1UL << 28) | OSXSAVEFlag);
+			const int FMAFlag = ((1UL << 12) | AVXFlag | OSXSAVEFlag);
+			if ((cpuInfo[2] & FMAFlag) == FMAFlag && (sseExt & 6) == 6)
+			{
+				capabilities |= btCpuFeatureUtility::CPU_FEATURE_FMA3;
+			}
+
+			const int SSE41Flag = (1 << 19);
+			if (cpuInfo[2] & SSE41Flag)
+			{
+				capabilities |= btCpuFeatureUtility::CPU_FEATURE_SSE4_1;
+			}
+		}
+#endif//BT_ALLOW_SSE4
+
+		testedCapabilities = true;
+		return capabilities;
+	}
+
+
+};
+
+
+#endif //BT_CPU_UTILITY_H

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

@@ -25,14 +25,14 @@ ATTRIBUTE_ALIGNED16(struct)	btDefaultMotionState : public btMotionState
 	///synchronizes world transform from user to physics
 	virtual void	getWorldTransform(btTransform& centerOfMassWorldTrans ) const 
 	{
-			centerOfMassWorldTrans = 	m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ;
+			centerOfMassWorldTrans = m_graphicsWorldTrans * m_centerOfMassOffset.inverse() ;
 	}
 
 	///synchronizes world transform from physics to user
 	///Bullet only calls the update of worldtransform for active objects
 	virtual void	setWorldTransform(const btTransform& centerOfMassWorldTrans)
 	{
-			m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ;
+			m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset;
 	}
 
 	

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

@@ -1,117 +1,130 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2011 Advanced Micro Devices, Inc.  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-#ifndef GRAHAM_SCAN_2D_CONVEX_HULL_H
-#define GRAHAM_SCAN_2D_CONVEX_HULL_H
-
-
-#include "btVector3.h"
-#include "btAlignedObjectArray.h"
-
-struct GrahamVector3 : public btVector3
-{
-	GrahamVector3(const btVector3& org, int orgIndex)
-		:btVector3(org),
-			m_orgIndex(orgIndex)
-	{
-	}
-	btScalar	m_angle;
-	int m_orgIndex;
-};
-
-
-struct btAngleCompareFunc {
-	btVector3 m_anchor;
-	btAngleCompareFunc(const btVector3& anchor)
-	: m_anchor(anchor) 
-	{
-	}
-	bool operator()(const GrahamVector3& a, const GrahamVector3& b) const {
-		if (a.m_angle != b.m_angle)
-			return a.m_angle < b.m_angle;
-		else
-		{
-			btScalar al = (a-m_anchor).length2();
-			btScalar bl = (b-m_anchor).length2();
-			if (al != bl)
-				return  al < bl;
-			else
-			{
-				return a.m_orgIndex < b.m_orgIndex;
-			}
-		}
-	}
-};
-
-inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector3>& originalPoints, btAlignedObjectArray<GrahamVector3>& hull, const btVector3& normalAxis)
-{
-	btVector3 axis0,axis1;
-	btPlaneSpace1(normalAxis,axis0,axis1);
-	
-
-	if (originalPoints.size()<=1)
-	{
-		for (int i=0;i<originalPoints.size();i++)
-			hull.push_back(originalPoints[0]);
-		return;
-	}
-	//step1 : find anchor point with smallest projection on axis0 and move it to first location
-	for (int i=0;i<originalPoints.size();i++)
-	{
-//		const btVector3& left = originalPoints[i];
-//		const btVector3& right = originalPoints[0];
-		btScalar projL = originalPoints[i].dot(axis0);
-		btScalar projR = originalPoints[0].dot(axis0);
-		if (projL < projR)
-		{
-			originalPoints.swap(0,i);
-		}
-	}
-
-	//also precompute angles
-	originalPoints[0].m_angle = -1e30f;
-	for (int i=1;i<originalPoints.size();i++)
-	{
-		btVector3 xvec = axis0;
-		btVector3 ar = originalPoints[i]-originalPoints[0];
-		originalPoints[i].m_angle = btCross(xvec, ar).dot(normalAxis) / ar.length();
-	}
-
-	//step 2: sort all points, based on 'angle' with this anchor
-	btAngleCompareFunc comp(originalPoints[0]);
-	originalPoints.quickSortInternal(comp,1,originalPoints.size()-1);
-
-	int i;
-	for (i = 0; i<2; i++) 
-		hull.push_back(originalPoints[i]);
-
-	//step 3: keep all 'convex' points and discard concave points (using back tracking)
-	for (; i != originalPoints.size(); i++) 
-	{
-		bool isConvex = false;
-		while (!isConvex&& hull.size()>1) {
-			btVector3& a = hull[hull.size()-2];
-			btVector3& b = hull[hull.size()-1];
-			isConvex = btCross(a-b,a-originalPoints[i]).dot(normalAxis)> 0;
-			if (!isConvex)
-				hull.pop_back();
-			else 
-				hull.push_back(originalPoints[i]);
-		}
-	}
-}
-
-#endif //GRAHAM_SCAN_2D_CONVEX_HULL_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2011 Advanced Micro Devices, Inc.  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef GRAHAM_SCAN_2D_CONVEX_HULL_H
+#define GRAHAM_SCAN_2D_CONVEX_HULL_H
+
+
+#include "btVector3.h"
+#include "btAlignedObjectArray.h"
+
+struct GrahamVector3 : public btVector3
+{
+	GrahamVector3(const btVector3& org, int orgIndex)
+		:btVector3(org),
+			m_orgIndex(orgIndex)
+	{
+	}
+	btScalar	m_angle;
+	int m_orgIndex;
+};
+
+
+struct btAngleCompareFunc {
+	btVector3 m_anchor;
+	btAngleCompareFunc(const btVector3& anchor)
+	: m_anchor(anchor) 
+	{
+	}
+	bool operator()(const GrahamVector3& a, const GrahamVector3& b) const {
+		if (a.m_angle != b.m_angle)
+			return a.m_angle < b.m_angle;
+		else
+		{
+			btScalar al = (a-m_anchor).length2();
+			btScalar bl = (b-m_anchor).length2();
+			if (al != bl)
+				return  al < bl;
+			else
+			{
+				return a.m_orgIndex < b.m_orgIndex;
+			}
+		}
+	}
+};
+
+inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector3>& originalPoints, btAlignedObjectArray<GrahamVector3>& hull, const btVector3& normalAxis)
+{
+	btVector3 axis0,axis1;
+	btPlaneSpace1(normalAxis,axis0,axis1);
+	
+
+	if (originalPoints.size()<=1)
+	{
+		for (int i=0;i<originalPoints.size();i++)
+			hull.push_back(originalPoints[0]);
+		return;
+	}
+	//step1 : find anchor point with smallest projection on axis0 and move it to first location
+	for (int i=0;i<originalPoints.size();i++)
+	{
+//		const btVector3& left = originalPoints[i];
+//		const btVector3& right = originalPoints[0];
+		btScalar projL = originalPoints[i].dot(axis0);
+		btScalar projR = originalPoints[0].dot(axis0);
+		if (projL < projR)
+		{
+			originalPoints.swap(0,i);
+		}
+	}
+
+	//also precompute angles
+	originalPoints[0].m_angle = -1e30f;
+	for (int i=1;i<originalPoints.size();i++)
+	{
+	    btVector3 ar = originalPoints[i]-originalPoints[0];
+	    btScalar ar1 = axis1.dot(ar);
+	    btScalar ar0 = axis0.dot(ar);
+	    if( ar1*ar1+ar0*ar0 < FLT_EPSILON ) 
+	    {
+	      originalPoints[i].m_angle = 0.0f;
+	    }
+	    else
+	    {
+	      originalPoints[i].m_angle = btAtan2Fast(ar1, ar0);
+	    }
+	}
+
+	//step 2: sort all points, based on 'angle' with this anchor
+	btAngleCompareFunc comp(originalPoints[0]);
+	originalPoints.quickSortInternal(comp,1,originalPoints.size()-1);
+
+	int i;
+	for (i = 0; i<2; i++) 
+		hull.push_back(originalPoints[i]);
+
+	//step 3: keep all 'convex' points and discard concave points (using back tracking)
+	for (; i != originalPoints.size(); i++) 
+	{
+		bool isConvex = false;
+		while (!isConvex&& hull.size()>1) {
+			btVector3& a = hull[hull.size()-2];
+			btVector3& b = hull[hull.size()-1];
+			isConvex = btCross(a-b,a-originalPoints[i]).dot(normalAxis)> 0;
+			if (!isConvex)
+				hull.pop_back();
+			else 
+				hull.push_back(originalPoints[i]);
+		}
+
+	    if( hull.size() == 1 )
+	    {
+	      hull.push_back( originalPoints[i] );
+	    }
+	}
+}
+
+#endif //GRAHAM_SCAN_2D_CONVEX_HULL_H

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

@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-// Modified by Lasse Oorni for Urho3D
 
 #ifndef BT_IDEBUG_DRAW__H
 #define BT_IDEBUG_DRAW__H
@@ -47,14 +46,12 @@ class	btIDebugDraw
 		DBG_DrawConstraints = (1 << 11),
 		DBG_DrawConstraintLimits = (1 << 12),
 		DBG_FastWireframe = (1<<13),
-        DBG_DrawNormals = (1<<14),
+		DBG_DrawNormals = (1<<14),
+		DBG_DrawFrames = (1<<15),
 		DBG_MAX_DEBUG_DRAW_MODE
 	};
 
 	virtual ~btIDebugDraw() {};
-	
-	// Urho3D: added function to test visibility of an AABB
-	virtual bool    isVisible(const btVector3& aabbMin,const btVector3& aabbMax)=0;
 
 	virtual void	drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
 		
@@ -151,7 +148,7 @@ class	btIDebugDraw
 		const btVector3& vx = axis;
 		btVector3 vy = normal.cross(axis);
 		btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
-		int nSteps = (int)((maxAngle - minAngle) / step);
+		int nSteps = (int)btFabs((maxAngle - minAngle) / step);
 		if(!nSteps) nSteps = 1;
 		btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle);
 		if(drawSect)
@@ -442,6 +439,10 @@ class	btIDebugDraw
 		drawLine(transform*pt0,transform*pt1,color);
 		drawLine(transform*pt2,transform*pt3,color);
 	}
+
+	virtual void flushLines()
+	{
+	}
 };
 
 

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

@@ -20,6 +20,12 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
+//#define BT_DEBUG_OSTREAM
+#ifdef BT_DEBUG_OSTREAM
+#include <iostream>
+#include <iomanip>      // std::setw
+#endif //BT_DEBUG_OSTREAM
+
 class btIntSortPredicate
 {
 	public:
@@ -30,6 +36,121 @@ class btIntSortPredicate
 };
 
 
+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();
+	}
+	
+	T nrm2() const
+	{
+		T norm = T(0);
+		
+		int nn = rows();
+		
+		{
+			if (nn == 1)
+			{
+				norm = btFabs((*this)[0]);
+			}
+			else
+			{
+				T scale = 0.0;
+				T ssq = 1.0;
+				
+				/* The following loop is equivalent to this call to the LAPACK
+				 auxiliary routine:   CALL SLASSQ( N, X, INCX, SCALE, SSQ ) */
+				
+				for (int ix=0;ix<nn;ix++)
+				{
+					if ((*this)[ix] != 0.0)
+					{
+						T absxi = btFabs((*this)[ix]);
+						if (scale < absxi)
+						{
+							T temp;
+							temp = scale / absxi;
+							ssq = ssq * (temp * temp) + 1.0;
+							scale = absxi;
+						}
+						else
+						{
+							T temp;
+							temp = absxi / scale;
+							ssq += temp * temp;
+						}
+					}
+				}
+				norm = scale * sqrt(ssq);
+			}
+		}
+		return norm;
+		
+	}
+	void	setZero()
+	{
+		if (m_storage.size())
+		{
+			//	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);
+ }
+ */
+
+
 template <typename T> 
 struct btMatrixX
 {
@@ -40,8 +161,7 @@ struct btMatrixX
 	int m_setElemOperations;
 
 	btAlignedObjectArray<T>	m_storage;
-	btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
-	btAlignedObjectArray< btAlignedObjectArray<int> > m_colNonZeroElements;
+	mutable btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
 
 	T* getBufferPointerWritable() 
 	{
@@ -78,7 +198,6 @@ struct btMatrixX
 			BT_PROFILE("m_storage.resize");
 			m_storage.resize(rows*cols);
 		}
-		clearSparseInfo();
 	}
 	int cols() const
 	{
@@ -109,49 +228,44 @@ struct btMatrixX
 		}
 	}
 	
+	
+	void setElem(int row,int col, T val)
+	{
+		m_setElemOperations++;
+		m_storage[row*m_cols+col] = val;
+	}
+	
+	void mulElem(int row,int col, T val)
+	{
+		m_setElemOperations++;
+		//mul doesn't change sparsity info
+
+		m_storage[row*m_cols+col] *= val;
+	}
+	
+	
+	
+	
 	void copyLowerToUpperTriangle()
 	{
 		int count=0;
-		for (int row=0;row<m_rowNonZeroElements1.size();row++)
+		for (int row=0;row<rows();row++)
 		{
-			for (int j=0;j<m_rowNonZeroElements1[row].size();j++)
+			for (int col=0;col<row;col++)
 			{
-				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()
 	{
@@ -162,12 +276,21 @@ struct btMatrixX
 			//for (int i=0;i<m_storage.size();i++)
 	//			m_storage[i]=0;
 		}
+	}
+	
+	void setIdentity()
+	{
+		btAssert(rows() == cols());
+		
+		setZero();
+		for (int row=0;row<rows();row++)
 		{
-			BT_PROFILE("clearSparseInfo=0");
-			clearSparseInfo();
+			setElem(row,row,1);
 		}
 	}
 
+	
+
 	void	printMatrix(const char* msg)
 	{
 		printf("%s ---------------------\n",msg);
@@ -182,28 +305,9 @@ struct btMatrixX
 		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()
+
+
+	void rowComputeNonZeroElements() const
 	{
 		m_rowNonZeroElements1.resize(rows());
 		for (int i=0;i<rows();i++)
@@ -218,13 +322,11 @@ struct btMatrixX
 			}
 		}
 	}
-	*/
 	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++)
 			{
@@ -234,37 +336,13 @@ struct btMatrixX
 					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
+		//btMatrixX*btMatrixX implementation, brute force
 		btAssert(cols() == other.rows());
 
 		btMatrixX res(rows(),other.cols());
@@ -272,76 +350,18 @@ struct btMatrixX
 //		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;
+//					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)
+//						bool useOtherCol = true;
 						{
-							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
+							for (int v=0;v<rows();v++)
 							{
-								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)
 								{
@@ -404,73 +424,61 @@ struct btMatrixX
 			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
+	
+	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const T value)
 	{
-		return m_storage[index];
+		int numRows = rowend+1-rowstart;
+		int numCols = colend+1-colstart;
+		
+		for (int row=0;row<numRows;row++)
+		{
+			for (int col=0;col<numCols;col++)
+			{
+				setElem(rowstart+row,colstart+col,value);
+			}
+		}
 	}
-
-	T& operator[] (int index)
+	
+	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btMatrixX& block)
 	{
-		return m_storage[index];
+		btAssert(rowend+1-rowstart == block.rows());
+		btAssert(colend+1-colstart == block.cols());
+		for (int row=0;row<block.rows();row++)
+		{
+			for (int col=0;col<block.cols();col++)
+			{
+				setElem(rowstart+row,colstart+col,block(row,col));
+			}
+		}
 	}
-
-	T* getBufferPointerWritable() 
+	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btVectorX<T>& block)
 	{
-		return m_storage.size() ? &m_storage[0] : 0;
+		btAssert(rowend+1-rowstart == block.rows());
+		btAssert(colend+1-colstart == block.cols());
+		for (int row=0;row<block.rows();row++)
+		{
+			for (int col=0;col<block.cols();col++)
+			{
+				setElem(rowstart+row,colstart+col,block[row]);
+			}
+		}
 	}
-
-	const T* getBufferPointer() const
+	
+	
+	btMatrixX negative()
 	{
-		return m_storage.size() ? &m_storage[0] : 0;
+		btMatrixX neg(rows(),cols());
+		for (int i=0;i<rows();i++)
+			for (int j=0;j<cols();j++)
+			{
+				T v = (*this)(i,j);
+				neg.setElem(i,j,-v);
+			}
+		return neg;
 	}
 
 };
-/*
-template <typename T> 
-void setElem(btMatrixX<T>& mat, int row, int col, T val)
-{
-	mat.setElem(row,col,val);
-}
-*/
+
 
 
 typedef btMatrixX<float> btMatrixXf;
@@ -480,6 +488,47 @@ typedef btMatrixX<double> btMatrixXd;
 typedef btVectorX<double> btVectorXd;
 
 
+#ifdef BT_DEBUG_OSTREAM
+template <typename T> 
+std::ostream& operator<< (std::ostream& os, const btMatrixX<T>& mat)
+	{
+		
+		os << " [";
+		//printf("%s ---------------------\n",msg);
+		for (int i=0;i<mat.rows();i++)
+		{
+			for (int j=0;j<mat.cols();j++)
+			{
+				os << std::setw(12) << mat(i,j);
+			}
+			if (i!=mat.rows()-1)
+				os << std::endl << "  ";
+		}
+		os << " ]";
+		//printf("\n---------------------\n");
+
+		return os;
+	}
+template <typename T> 
+std::ostream& operator<< (std::ostream& os, const btVectorX<T>& mat)
+	{
+		
+		os << " [";
+		//printf("%s ---------------------\n",msg);
+		for (int i=0;i<mat.rows();i++)
+		{
+				os << std::setw(12) << mat[i];
+			if (i!=mat.rows()-1)
+				os << std::endl << "  ";
+		}
+		os << " ]";
+		//printf("\n---------------------\n");
+
+		return os;
+	}
+
+#endif //BT_DEBUG_OSTREAM
+
 
 inline void setElem(btMatrixXd& mat, int row, int col, double val)
 {

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

@@ -10,7 +10,7 @@
 **
 ***************************************************************************************************/
 
-// Credits: The Clock class was inspired by the Timer classes in 
+// Credits: The Clock class was inspired by the Timer classes in
 // Ogre (www.ogre3d.org).
 
 #include "btQuickprof.h"
@@ -27,8 +27,8 @@ static btClock gProfileClock;
 #include <stdio.h>
 #endif
 
-#if defined (SUNOS) || defined (__SUNOS__) 
-#include <stdio.h> 
+#if defined (SUNOS) || defined (__SUNOS__)
+#include <stdio.h>
 #endif
 
 #if defined(WIN32) || defined(_WIN32)
@@ -37,12 +37,17 @@ static btClock gProfileClock;
 #define WIN32_LEAN_AND_MEAN
 #define NOWINRES
 #define NOMCX
-#define NOIME 
+#define NOIME
 
 #ifdef _XBOX
 	#include <Xtl.h>
 #else //_XBOX
 	#include <windows.h>
+
+#if WINVER < 0x0600
+ULONGLONG GetTickCount64() { return GetTickCount(); }
+#endif
+
 #endif //_XBOX
 
 #include <time.h>
@@ -59,7 +64,7 @@ struct btClockData
 
 #ifdef BT_USE_WINDOWS_TIMERS
 	LARGE_INTEGER mClockFrequency;
-	DWORD mStartTick;
+	LONGLONG mStartTick;
 	LONGLONG mPrevElapsedTime;
 	LARGE_INTEGER mStartTime;
 #else
@@ -105,7 +110,7 @@ void btClock::reset()
 {
 #ifdef BT_USE_WINDOWS_TIMERS
 	QueryPerformanceCounter(&m_data->mStartTime);
-	m_data->mStartTick = GetTickCount();
+	m_data->mStartTick = GetTickCount64();
 	m_data->mPrevElapsedTime = 0;
 #else
 #ifdef __CELLOS_LV2__
@@ -121,34 +126,34 @@ void btClock::reset()
 #endif
 }
 
-/// Returns the time in ms since the last call to reset or since 
+/// Returns the time in ms since the last call to reset or since
 /// the btClock was created.
 unsigned long int btClock::getTimeMilliseconds()
 {
 #ifdef BT_USE_WINDOWS_TIMERS
 	LARGE_INTEGER currentTime;
 	QueryPerformanceCounter(&currentTime);
-	LONGLONG elapsedTime = currentTime.QuadPart - 
+	LONGLONG elapsedTime = currentTime.QuadPart -
 		m_data->mStartTime.QuadPart;
 		// Compute the number of millisecond ticks elapsed.
-	unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / 
+	unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
 		m_data->mClockFrequency.QuadPart);
-		// Check for unexpected leaps in the Win32 performance counter.  
-	// (This is caused by unexpected data across the PCI to ISA 
+		// Check for unexpected leaps in the Win32 performance counter.
+		// (This is caused by unexpected data across the PCI to ISA
 		// bridge, aka south bridge.  See Microsoft KB274323.)
-		unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick;
+		unsigned long elapsedTicks = (unsigned long)(GetTickCount64() - m_data->mStartTick);
 		signed long msecOff = (signed long)(msecTicks - elapsedTicks);
 		if (msecOff < -100 || msecOff > 100)
 		{
 			// Adjust the starting time forwards.
-			LONGLONG msecAdjustment = mymin(msecOff * 
-				m_data->mClockFrequency.QuadPart / 1000, elapsedTime - 
+			LONGLONG msecAdjustment = mymin(msecOff *
+				m_data->mClockFrequency.QuadPart / 1000, elapsedTime -
 				m_data->mPrevElapsedTime);
 			m_data->mStartTime.QuadPart += msecAdjustment;
 			elapsedTime -= msecAdjustment;
 
 			// Recompute the number of millisecond ticks elapsed.
-			msecTicks = (unsigned long)(1000 * elapsedTime / 
+			msecTicks = (unsigned long)(1000 * elapsedTime /
 				m_data->mClockFrequency.QuadPart);
 		}
 
@@ -171,36 +176,36 @@ unsigned long int btClock::getTimeMilliseconds()
 
 		struct timeval currentTime;
 		gettimeofday(&currentTime, 0);
-		return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000 + 
+		return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000 +
 			(currentTime.tv_usec - m_data->mStartTime.tv_usec) / 1000;
 #endif //__CELLOS_LV2__
 #endif
 }
 
-	/// Returns the time in us since the last call to reset or since 
+	/// Returns the time in us since the last call to reset or since
 	/// the Clock was created.
 unsigned long int btClock::getTimeMicroseconds()
 {
 #ifdef BT_USE_WINDOWS_TIMERS
 		LARGE_INTEGER currentTime;
 		QueryPerformanceCounter(&currentTime);
-		LONGLONG elapsedTime = currentTime.QuadPart - 
+		LONGLONG elapsedTime = currentTime.QuadPart -
 			m_data->mStartTime.QuadPart;
 
 		// Compute the number of millisecond ticks elapsed.
-		unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / 
+		unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
 			m_data->mClockFrequency.QuadPart);
 
-		// Check for unexpected leaps in the Win32 performance counter.  
-		// (This is caused by unexpected data across the PCI to ISA 
+		// Check for unexpected leaps in the Win32 performance counter.
+		// (This is caused by unexpected data across the PCI to ISA
 		// bridge, aka south bridge.  See Microsoft KB274323.)
-		unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick;
+		unsigned long elapsedTicks = (unsigned long)(GetTickCount64() - m_data->mStartTick);
 		signed long msecOff = (signed long)(msecTicks - elapsedTicks);
 		if (msecOff < -100 || msecOff > 100)
 		{
 			// Adjust the starting time forwards.
-			LONGLONG msecAdjustment = mymin(msecOff * 
-				m_data->mClockFrequency.QuadPart / 1000, elapsedTime - 
+			LONGLONG msecAdjustment = mymin(msecOff *
+				m_data->mClockFrequency.QuadPart / 1000, elapsedTime -
 				m_data->mPrevElapsedTime);
 			m_data->mStartTime.QuadPart += msecAdjustment;
 			elapsedTime -= msecAdjustment;
@@ -210,7 +215,7 @@ unsigned long int btClock::getTimeMicroseconds()
 		m_data->mPrevElapsedTime = elapsedTime;
 
 		// Convert to microseconds.
-		unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / 
+		unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime /
 			m_data->mClockFrequency.QuadPart);
 
 		return usecTicks;
@@ -229,14 +234,22 @@ unsigned long int btClock::getTimeMicroseconds()
 
 		struct timeval currentTime;
 		gettimeofday(&currentTime, 0);
-		return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 + 
+		return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 +
 			(currentTime.tv_usec - m_data->mStartTime.tv_usec);
 #endif//__CELLOS_LV2__
-#endif 
+#endif
 }
 
 
 
+/// Returns the time in s since the last call to reset or since 
+/// the Clock was created.
+btScalar btClock::getTimeSeconds()
+{
+	static const btScalar microseconds_to_seconds = btScalar(0.000001);
+	return btScalar(getTimeMicroseconds()) * microseconds_to_seconds;
+}
+
 
 
 inline void Profile_Get_Ticks(unsigned long int * ticks)
@@ -293,8 +306,7 @@ void	CProfileNode::CleanupMemory()
 
 CProfileNode::~CProfileNode( void )
 {
-	delete ( Child);
-	delete ( Sibling);
+	CleanupMemory();
 }
 
 
@@ -318,7 +330,7 @@ CProfileNode * CProfileNode::Get_Sub_Node( const char * name )
 	}
 
 	// We didn't find it, so add it
-	
+
 	CProfileNode * node = new CProfileNode( name, this );
 	node->Sibling = Child;
 	Child = node;
@@ -330,7 +342,7 @@ void	CProfileNode::Reset( void )
 {
 	TotalCalls = 0;
 	TotalTime = 0.0f;
-	
+
 
 	if ( Child ) {
 		Child->Reset();
@@ -352,7 +364,7 @@ void	CProfileNode::Call( void )
 
 bool	CProfileNode::Return( void )
 {
-	if ( --RecursionCounter == 0 && TotalCalls != 0 ) { 
+	if ( --RecursionCounter == 0 && TotalCalls != 0 ) {
 		unsigned long int time;
 		Profile_Get_Ticks(&time);
 		time-=StartTime;
@@ -445,8 +457,8 @@ void	CProfileManager::Start_Profile( const char * name )
 {
 	if (name != CurrentNode->Get_Name()) {
 		CurrentNode = CurrentNode->Get_Sub_Node( name );
-	} 
-	
+	}
+
 	CurrentNode->Call();
 }
 
@@ -470,7 +482,7 @@ void	CProfileManager::Stop_Profile( void )
  *    This resets everything except for the tree structure.  All of the timing data is reset.  *
  *=============================================================================================*/
 void	CProfileManager::Reset( void )
-{ 
+{
 	gProfileClock.reset();
 	Root.Reset();
     Root.Call();
@@ -516,9 +528,9 @@ void	CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spaci
 	printf("Profiling: %s (total running time: %.3f ms) ---\n",	profileIterator->Get_Current_Parent_Name(), parent_time );
 	float totalTime = 0.f;
 
-	
+
 	int numChildren = 0;
-	
+
 	for (i = 0; !profileIterator->Is_Done(); i++,profileIterator->Next())
 	{
 		numChildren++;
@@ -535,11 +547,11 @@ void	CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spaci
 
 	if (parent_time < accumulated_time)
 	{
-		printf("what's wrong\n");
+		//printf("what's wrong\n");
 	}
 	for (i=0;i<spacing;i++)	printf(".");
 	printf("%s (%.3f %%) :: %.3f ms\n", "Unaccounted:",parent_time > SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time);
-	
+
 	for (i=0;i<numChildren;i++)
 	{
 		profileIterator->Enter_Child(i);

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

@@ -16,8 +16,7 @@
 #define BT_QUICK_PROF_H
 
 //To disable built-in profiling, please comment out next line
-// Urho3D: disable profiling, as Urho has its own hierarchic profiler
-#define BT_NO_PROFILE 1
+//#define BT_NO_PROFILE 1
 #ifndef BT_NO_PROFILE
 #include <stdio.h>//@todo remove this, backwards compatibility
 #include "btScalar.h"
@@ -53,6 +52,11 @@ public:
 	/// Returns the time in us since the last call to reset or since 
 	/// the Clock was created.
 	unsigned long int getTimeMicroseconds();
+	
+	/// Returns the time in s since the last call to reset or since 
+	/// the Clock was created.
+	btScalar getTimeSeconds();
+	
 private:
 	struct btClockData* m_data;
 };

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

@@ -12,7 +12,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-// Modified by Lasse Oorni for Urho3D
 
 
 #ifndef BT_SCALAR_H
@@ -29,7 +28,7 @@ subject to the following restrictions:
 #include <float.h>
 
 /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
-#define BT_BULLET_VERSION 282
+#define BT_BULLET_VERSION 283
 
 inline int	btGetVersion()
 {
@@ -49,6 +48,11 @@ inline int	btGetVersion()
 			#define ATTRIBUTE_ALIGNED16(a) a
 			#define ATTRIBUTE_ALIGNED64(a) a
 			#define ATTRIBUTE_ALIGNED128(a) a
+		#elif (_M_ARM)
+			#define SIMD_FORCE_INLINE __forceinline
+			#define ATTRIBUTE_ALIGNED16(a) __declspec() a
+			#define ATTRIBUTE_ALIGNED64(a) __declspec() a
+			#define ATTRIBUTE_ALIGNED128(a) __declspec () a
 		#else
 			//#define BT_HAS_ALIGNED_ALLOCATOR
 			#pragma warning(disable : 4324) // disable padding warning
@@ -68,14 +72,20 @@ inline int	btGetVersion()
  			#define btFsel(a,b,c) __fsel((a),(b),(c))
 		#else
 
-// Urho3D: allow to disable SSE
-#if (defined (ATOMIC_SSE) && defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
+#if defined (_M_ARM)
+            //Do not turn SSE on for ARM (may want to turn on BT_USE_NEON however)
+#elif (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
 			#if _MSC_VER>1400
 				#define BT_USE_SIMD_VECTOR3
 			#endif
 
 			#define BT_USE_SSE
 			#ifdef BT_USE_SSE
+
+#if (_MSC_FULL_VER >= 170050727)//Visual Studio 2012 can compile SSE4/FMA3 (but SSE4/FMA3 is not enabled by default)
+			#define BT_ALLOW_SSE4
+#endif //(_MSC_FULL_VER >= 160040219)
+
 			//BT_USE_SSE_IN_API is disabled under Windows by default, because 
 			//it makes it harder to integrate Bullet into your application under Windows 
 			//(structured embedding Bullet structs/classes need to be 16-byte aligned)
@@ -163,8 +173,7 @@ inline int	btGetVersion()
 #else
 	//non-windows systems
 
-// Urho3D: allow to disable SSE
-#if (defined (ATOMIC_SSE) && defined (__APPLE__) && (!defined (BT_USE_DOUBLE_PRECISION)))
+#if (defined (__APPLE__) && (!defined (BT_USE_DOUBLE_PRECISION)))
     #if defined (__i386__) || defined (__x86_64__)
 		#define BT_USE_SIMD_VECTOR3
 		#define BT_USE_SSE
@@ -287,6 +296,10 @@ static int btNanMask = 0x7F800001;
 #ifndef BT_INFINITY
 static  int btInfinityMask = 0x7F800000;
 #define BT_INFINITY (*(float*)&btInfinityMask)
+inline int btGetInfinityMask()//suppress stupid compiler warning
+{
+	return btInfinityMask;
+}
 #endif
 
 //use this, in case there are clashes (such as xnamath.h)
@@ -337,8 +350,23 @@ inline __m128 operator * (const __m128 A, const __m128 B)
 #else//BT_USE_NEON
 
 	#ifndef BT_INFINITY
-	static  int btInfinityMask = 0x7F800000;
-	#define BT_INFINITY (*(float*)&btInfinityMask)
+		struct btInfMaskConverter
+		{
+		        union {
+		                float mask;
+		                int intmask;
+		        };
+		        btInfMaskConverter(int mask=0x7F800000)
+		        :intmask(mask)
+		        {
+		        }
+		};
+		static btInfMaskConverter btInfinityMask = 0x7F800000;
+		#define BT_INFINITY (btInfinityMask.mask)
+		inline int btGetInfinityMask()//suppress stupid compiler warning
+		{
+		        return btInfinityMask.intmask;
+		}
 	#endif
 #endif//BT_USE_NEON
 
@@ -390,19 +418,30 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); }
 SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) 
 { 
 #ifdef USE_APPROXIMATION
+#ifdef __LP64__
+    float xhalf = 0.5f*y;
+    int i = *(int*)&y;
+    i = 0x5f375a86 - (i>>1);
+    y = *(float*)&i;
+    y = y*(1.5f - xhalf*y*y);
+    y = y*(1.5f - xhalf*y*y);
+    y = y*(1.5f - xhalf*y*y);
+    y=1/y;
+    return y;
+#else
     double x, z, tempf;
     unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
-
-	tempf = y;
-	*tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */
-	x =  tempf;
-	z =  y*btScalar(0.5);
-	x = (btScalar(1.5)*x)-(x*x)*(x*z);         /* iteration formula     */
-	x = (btScalar(1.5)*x)-(x*x)*(x*z);
-	x = (btScalar(1.5)*x)-(x*x)*(x*z);
-	x = (btScalar(1.5)*x)-(x*x)*(x*z);
-	x = (btScalar(1.5)*x)-(x*x)*(x*z);
-	return x*y;
+    tempf = y;
+    *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */
+    x =  tempf;
+    z =  y*btScalar(0.5);
+    x = (btScalar(1.5)*x)-(x*x)*(x*z);         /* iteration formula     */
+    x = (btScalar(1.5)*x)-(x*x)*(x*z);
+    x = (btScalar(1.5)*x)-(x*x)*(x*z);
+    x = (btScalar(1.5)*x)-(x*x)*(x*z);
+    x = (btScalar(1.5)*x)-(x*x)*(x*z);
+    return x*y;
+#endif
 #else
 	return sqrtf(y); 
 #endif
@@ -435,7 +474,7 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
 #endif
 
 #define SIMD_PI           btScalar(3.1415926535897932384626433832795029)
-#define SIMD_2_PI         btScalar(2.0) * SIMD_PI
+#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)
@@ -731,4 +770,5 @@ template <typename T>T* btAlignPointer(T* unalignedPtr, size_t alignment)
 	return converter.ptr;
 }
 
+
 #endif //BT_SCALAR_H

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

@@ -1,5 +1,5 @@
 char sBulletDNAstr[]= {
-char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(69),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109),
+char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(99),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109),
 char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95),
 char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111),
 char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110),
@@ -162,340 +162,390 @@ char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char
 char(97),char(98),char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105),
 char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(83),
 char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),
-char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65),
-char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109),
-char(95),char(116),char(97),char(117),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97),
-char(120),char(69),char(114),char(114),char(111),char(114),char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111),
-char(114),char(0),char(109),char(95),char(101),char(114),char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111),
-char(98),char(97),char(108),char(67),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),
-char(101),char(80),char(101),char(110),char(101),char(116),char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),
-char(100),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110),
-char(69),char(114),char(112),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119),
-char(97),char(114),char(109),char(115),char(116),char(97),char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),
-char(109),char(97),char(120),char(71),char(121),char(114),char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109),
-char(95),char(115),char(105),char(110),char(103),char(108),char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),
-char(105),char(99),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117),
-char(109),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),
-char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99),
-char(116),char(82),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),
-char(100),char(0),char(109),char(95),char(109),char(105),char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116),
-char(99),char(104),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),
-char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),
-char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),
-char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95),
-char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),
-char(109),char(95),char(112),char(114),char(101),char(118),char(105),char(111),char(117),char(115),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),
-char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97),
-char(116),char(101),char(100),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95),
-char(97),char(114),char(101),char(97),char(0),char(109),char(95),char(97),char(116),char(116),char(97),char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101),
-char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110),
-char(103),char(116),char(104),char(0),char(109),char(95),char(98),char(98),char(101),char(110),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100),
-char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114),
-char(101),char(97),char(0),char(109),char(95),char(99),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),
-char(105),char(99),char(101),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101),
-char(0),char(109),char(95),char(99),char(49),char(0),char(109),char(95),char(99),char(50),char(0),char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111),
-char(99),char(97),char(108),char(70),char(114),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100),
-char(121),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111),
-char(77),char(111),char(100),char(101),char(108),char(0),char(109),char(95),char(98),char(97),char(117),char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95),
-char(100),char(114),char(97),char(103),char(0),char(109),char(95),char(108),char(105),char(102),char(116),char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117),
-char(114),char(101),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105),
-char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99),
-char(104),char(0),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),
-char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(107),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99),
-char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116),
-char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),
-char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),
-char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),
-char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),
-char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),
-char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),
-char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),
-char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),
-char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102),
-char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),
-char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(109),char(97),char(120),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116),
-char(105),char(109),char(101),char(83),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73),
-char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),
-char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116),
-char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116),
-char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97),
-char(108),char(101),char(0),char(109),char(95),char(97),char(113),char(113),char(0),char(109),char(95),char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111),
-char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(42),char(109),char(95),char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109),
-char(95),char(110),char(117),char(109),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87),
-char(101),char(105),char(103),char(116),char(115),char(0),char(109),char(95),char(98),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102),
-char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95),
-char(108),char(111),char(99),char(105),char(105),char(0),char(109),char(95),char(105),char(110),char(118),char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112),
-char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),
-char(91),char(50),char(93),char(0),char(109),char(95),char(108),char(118),char(0),char(109),char(95),char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97),
-char(109),char(101),char(114),char(101),char(102),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),
-char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97),
-char(109),char(101),char(82),char(101),char(102),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95),
-char(110),char(117),char(109),char(77),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109),
-char(95),char(105),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(110),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),
-char(109),char(95),char(110),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112),
-char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97),
-char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(116),char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109),
-char(97),char(120),char(83),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),
-char(115),char(101),char(0),char(109),char(95),char(115),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),
-char(112),char(117),char(108),char(115),char(101),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105),
-char(110),char(115),char(65),char(110),char(99),char(104),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109),
-char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(110),char(100),char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100),
-char(121),char(65),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50),
-char(93),char(0),char(109),char(95),char(99),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101),
-char(108),char(101),char(116),char(101),char(0),char(109),char(95),char(114),char(101),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50),
-char(93),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121),
-char(66),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109),
-char(95),char(112),char(111),char(115),char(101),char(0),char(42),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),
-char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109),
-char(95),char(102),char(97),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(116),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),
-char(0),char(42),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116),
-char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),
-char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0),
-char(109),char(95),char(110),char(117),char(109),char(70),char(97),char(99),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114),
-char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(109),char(95),char(110),char(117),char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0),
-char(109),char(95),char(110),char(117),char(109),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74),
-char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(102),char(105),char(103),char(0),char(0),char(84),char(89),char(80),char(69),
-char(87),char(0),char(0),char(0),char(99),char(104),char(97),char(114),char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),
-char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0),char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),
-char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116),char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),
-char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114),char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),
-char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101),char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),
-char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
-char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
-char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),
-char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),
-char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),
-char(97),char(116),char(97),char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),
-char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),
-char(111),char(100),char(101),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),
-char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),
-char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),
-char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),
-char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),
-char(66),char(118),char(104),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),
-char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),
-char(116),char(105),char(99),char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
-char(67),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),
-char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),
-char(105),char(117),char(115),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),
-char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),
-char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),
-char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),
-char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),
-char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),
-char(114),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),
-char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),
-char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),
-char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),
-char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),
-char(100),char(83),char(104),char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),
-char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),
-char(108),char(105),char(110),char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),
-char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108),
-char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),
-char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77),
-char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),
-char(120),char(72),char(117),char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),
-char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),
-char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),
-char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),
-char(87),char(111),char(114),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),
-char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),
-char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),
-char(100),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),
-char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),
-char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),
-char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49),
-char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),
-char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),
-char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),
-char(121),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),
-char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),
-char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),
-char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),
+char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(66),
+char(111),char(117),char(110),char(99),char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),
+char(80),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),
+char(108),char(105),char(110),char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(108),char(105),char(110),
+char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),
+char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(108),char(105),char(110),
+char(101),char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(108),
+char(105),char(110),char(101),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(108),
+char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),
+char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),
+char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105),
+char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(110),char(97),char(98),
+char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),
+char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),
+char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(97),
+char(110),char(103),char(117),char(108),char(97),char(114),char(66),char(111),char(117),char(110),char(99),char(101),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),
+char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),
+char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(77),char(111),char(116),char(111),
+char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(67),
+char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),
+char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(77),char(97),char(120),char(77),
+char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),
+char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),
+char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),
+char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),
+char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105),char(117),char(109),char(80),
+char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),
+char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(101),
+char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),
+char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(112),
+char(97),char(100),char(100),char(105),char(110),char(103),char(50),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(111),char(116),char(97),char(116),char(101),char(79),
+char(114),char(100),char(101),char(114),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65),char(0),char(109),char(95),char(97),char(120),char(105),
+char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109),char(95),char(116),char(97),char(117),char(0),char(109),
+char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97),char(120),char(69),char(114),char(114),char(111),char(114),
+char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111),char(114),char(0),char(109),char(95),char(101),char(114),
+char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111),char(98),char(97),char(108),char(67),char(102),char(109),
+char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(80),char(101),char(110),char(101),char(116),
+char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(115),char(112),
+char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110),char(69),char(114),char(112),char(0),char(109),char(95),
+char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119),char(97),char(114),char(109),char(115),char(116),char(97),
+char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(97),char(120),char(71),char(121),char(114),
+char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(115),char(105),char(110),char(103),char(108),
+char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),
+char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117),char(109),char(73),char(116),char(101),char(114),char(97),
+char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(77),char(111),char(100),char(101),char(0),char(109),
+char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(82),char(101),char(115),char(116),char(105),
+char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(109),char(105),
+char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116),char(99),char(104),char(83),char(105),char(122),char(101),
+char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(105),
+char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117),
+char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109),
+char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),
+char(97),char(108),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(114),char(101),char(118),
+char(105),char(111),char(117),char(115),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),
+char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97),char(116),char(101),char(100),char(70),char(111),char(114),
+char(99),char(101),char(0),char(109),char(95),char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95),char(97),char(114),char(101),char(97),char(0),char(109),
+char(95),char(97),char(116),char(116),char(97),char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),
+char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110),char(103),char(116),char(104),char(0),char(109),char(95),
+char(98),char(98),char(101),char(110),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),
+char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114),char(101),char(97),char(0),char(109),char(95),char(99),
+char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(52),
+char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(99),char(49),char(0),
+char(109),char(95),char(99),char(50),char(0),char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(70),char(114),char(97),
+char(109),char(101),char(0),char(42),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(0),char(109),char(95),char(110),char(111),
+char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111),char(77),char(111),char(100),char(101),char(108),char(0),
+char(109),char(95),char(98),char(97),char(117),char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95),char(100),char(114),char(97),char(103),char(0),char(109),
+char(95),char(108),char(105),char(102),char(116),char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117),char(114),char(101),char(0),char(109),char(95),char(118),
+char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105),char(99),char(70),char(114),char(105),char(99),char(116),
+char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99),char(104),char(0),char(109),char(95),char(114),char(105),
+char(103),char(105),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),
+char(95),char(107),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),
+char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),
+char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(72),char(97),char(114),char(100),char(110),char(101),
+char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101),
+char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101),
+char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),
+char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),
+char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115),
+char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),
+char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),
+char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),
+char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),
+char(95),char(109),char(97),char(120),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(99),char(97),
+char(108),char(101),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73),char(116),char(101),char(114),char(97),char(116),char(105),
+char(111),char(110),char(115),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(73),char(116),char(101),char(114),char(97),char(116),
+char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),
+char(110),char(115),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),
+char(110),char(115),char(0),char(109),char(95),char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(97),
+char(113),char(113),char(0),char(109),char(95),char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),
+char(115),char(0),char(42),char(109),char(95),char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(80),char(111),
+char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87),char(101),char(105),char(103),char(116),char(115),char(0),
+char(109),char(95),char(98),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102),char(114),char(97),char(109),char(101),char(0),char(109),
+char(95),char(102),char(114),char(97),char(109),char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(108),char(111),char(99),char(105),char(105),char(0),
+char(109),char(95),char(105),char(110),char(118),char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91),
+char(50),char(93),char(0),char(109),char(95),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),
+char(108),char(118),char(0),char(109),char(95),char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(114),char(101),char(102),char(115),
+char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109),
+char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97),char(109),char(101),char(82),char(101),char(102),char(115),
+char(0),char(109),char(95),char(110),char(117),char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(115),
+char(115),char(101),char(115),char(0),char(109),char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(105),char(109),char(97),char(115),char(115),
+char(0),char(109),char(95),char(110),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(105),char(109),
+char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),
+char(108),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),
+char(109),char(95),char(109),char(97),char(116),char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(120),char(83),char(101),char(108),char(102),
+char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(115),
+char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(70),
+char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105),char(110),char(115),char(65),char(110),char(99),char(104),
+char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),
+char(101),char(114),char(73),char(110),char(100),char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(0),char(42),char(109),char(95),
+char(98),char(111),char(100),char(121),char(66),char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(99),char(102),
+char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101),char(108),char(101),char(116),char(101),char(0),char(109),
+char(95),char(114),char(101),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50),char(93),char(0),char(109),char(95),char(98),char(111),
+char(100),char(121),char(65),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(116),char(121),char(112),char(101),char(0),
+char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(101),char(0),
+char(42),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100),
+char(101),char(115),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109),char(95),char(102),char(97),char(99),char(101),char(115),
+char(0),char(42),char(109),char(95),char(116),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(42),char(109),char(95),char(97),char(110),
+char(99),char(104),char(111),char(114),char(115),char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(42),char(109),
+char(95),char(106),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(116),char(101),char(114),char(105),char(97),
+char(108),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),
+char(97),char(99),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),
+char(0),char(109),char(95),char(110),char(117),char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(67),
+char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74),char(111),char(105),char(110),char(116),char(115),char(0),
+char(109),char(95),char(99),char(111),char(110),char(102),char(105),char(103),char(0),char(0),char(0),char(0),char(84),char(89),char(80),char(69),char(89),char(0),char(0),char(0),
+char(99),char(104),char(97),char(114),char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),char(0),char(117),char(115),char(104),
+char(111),char(114),char(116),char(0),char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),char(110),char(103),char(0),char(102),
+char(108),char(111),char(97),char(116),char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),char(0),char(80),char(111),char(105),
+char(110),char(116),char(101),char(114),char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),char(105),char(99),char(115),char(83),
+char(121),char(115),char(116),char(101),char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),char(116),char(86),char(101),char(99),
+char(116),char(111),char(114),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(86),char(101),char(99),char(116),
+char(111),char(114),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114),
+char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),
+char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),
+char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),
+char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(70),
+char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),
+char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),
+char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97),char(116),
+char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(68),
+char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),
+char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99),char(80),
+char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),
+char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115),char(0),
+char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),
+char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116),
+char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),
+char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68),char(97),
+char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110),char(116),
+char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),
+char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),
+char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),
+char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),
+char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),
+char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),
+char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110),char(100),
+char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83),char(104),
+char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104),char(97),
+char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),
+char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104),char(83),
+char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117),char(108),
+char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),
+char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),
+char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111),char(97),
+char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),
+char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),
+char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),
+char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70),char(108),char(111),
+char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),
+char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),
+char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),
+char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),
+char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49),char(0),char(98),char(116),char(84),
+char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),
+char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),
+char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97),char(116),
+char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),
+char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),
+char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),
+char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),
+char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),
+char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),
+char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),
+char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),
+char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),
+char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(67),char(111),
+char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),
+char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),
+char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),
+char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),
+char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),
+char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),
+char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),
+char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),
+char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),
+char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),
+char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),
+char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),
+char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),
+char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),
+char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),
 char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),
-char(50),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),
-char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),
-char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),
-char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),
-char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),
-char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),
-char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),
-char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),
-char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
-char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),
-char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),
-char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),
-char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),
-char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),
-char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),
-char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(83),char(108),
-char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),
-char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),
-char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),
-char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),
-char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),
-char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116),
-char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83),
-char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),
-char(66),char(111),char(100),char(121),char(70),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),
-char(121),char(84),char(101),char(116),char(114),char(97),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),
-char(65),char(110),char(99),char(104),char(111),char(114),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),
-char(111),char(110),char(102),char(105),char(103),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111),
-char(115),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116),
-char(101),char(114),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105),
-char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111),
-char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),
-char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0),char(12),char(0),char(36),char(0),char(8),char(0),char(16),char(0),
-char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(84),char(0),
-char(-124),char(0),char(12),char(0),char(52),char(0),char(52),char(0),char(20),char(0),char(64),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),
-char(32),char(0),char(28),char(0),char(60),char(0),char(56),char(0),char(76),char(0),char(76),char(0),char(24),char(0),char(60),char(0),char(60),char(0),char(60),char(0),
-char(16),char(0),char(64),char(0),char(68),char(0),char(-48),char(1),char(0),char(1),char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-24),char(1),
-char(-96),char(3),char(8),char(0),char(52),char(0),char(52),char(0),char(0),char(0),char(68),char(0),char(84),char(0),char(-124),char(0),char(116),char(0),char(92),char(1),
-char(-36),char(0),char(-116),char(1),char(124),char(1),char(-44),char(0),char(-4),char(0),char(-52),char(1),char(92),char(1),char(116),char(2),char(-52),char(0),char(108),char(1),
-char(92),char(0),char(-116),char(0),char(16),char(0),char(100),char(0),char(20),char(0),char(36),char(0),char(100),char(0),char(92),char(0),char(104),char(0),char(-64),char(0),
-char(92),char(1),char(104),char(0),char(-84),char(1),char(0),char(0),char(83),char(84),char(82),char(67),char(76),char(0),char(0),char(0),char(10),char(0),char(3),char(0),
-char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0),char(10),char(0),char(3),char(0),
-char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0),char(9),char(0),char(7),char(0),
-char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(15),char(0),char(1),char(0),
-char(13),char(0),char(9),char(0),char(16),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(17),char(0),char(2),char(0),char(15),char(0),char(10),char(0),
-char(13),char(0),char(11),char(0),char(18),char(0),char(2),char(0),char(16),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(19),char(0),char(4),char(0),
-char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(20),char(0),char(6),char(0),
-char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),
-char(0),char(0),char(21),char(0),char(21),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0),
-char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(22),char(0),char(3),char(0),char(2),char(0),char(14),char(0),
-char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(23),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0),
-char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),
-char(20),char(0),char(30),char(0),char(22),char(0),char(31),char(0),char(19),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),
-char(24),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0),
-char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(21),char(0),char(30),char(0),char(22),char(0),char(31),char(0),
-char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(19),char(0),char(32),char(0),char(25),char(0),char(3),char(0),char(0),char(0),char(35),char(0),
-char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(26),char(0),char(5),char(0),char(25),char(0),char(38),char(0),char(13),char(0),char(39),char(0),
-char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(27),char(0),char(5),char(0),char(25),char(0),char(38),char(0),
-char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(28),char(0),char(2),char(0),
-char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(29),char(0),char(4),char(0),char(27),char(0),char(47),char(0),char(28),char(0),char(48),char(0),
-char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(30),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(31),char(0),char(2),char(0),
-char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(32),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),
-char(33),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(34),char(0),char(8),char(0),char(13),char(0),char(54),char(0),
-char(14),char(0),char(55),char(0),char(30),char(0),char(56),char(0),char(32),char(0),char(57),char(0),char(33),char(0),char(58),char(0),char(31),char(0),char(59),char(0),
-char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(35),char(0),char(4),char(0),char(34),char(0),char(62),char(0),char(13),char(0),char(63),char(0),
-char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(36),char(0),char(7),char(0),char(25),char(0),char(38),char(0),char(35),char(0),char(65),char(0),
-char(23),char(0),char(66),char(0),char(24),char(0),char(67),char(0),char(37),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),
-char(38),char(0),char(2),char(0),char(36),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(39),char(0),char(4),char(0),char(17),char(0),char(71),char(0),
-char(25),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(40),char(0),char(4),char(0),char(25),char(0),char(38),char(0),
-char(39),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(41),char(0),char(3),char(0),char(27),char(0),char(47),char(0),
-char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(42),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(78),char(0),
-char(0),char(0),char(37),char(0),char(43),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),
-char(44),char(0),char(4),char(0),char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0),
-char(37),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(44),char(0),char(85),char(0),char(4),char(0),char(86),char(0),
-char(7),char(0),char(87),char(0),char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0),
-char(4),char(0),char(92),char(0),char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0),
-char(45),char(0),char(5),char(0),char(25),char(0),char(38),char(0),char(35),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0),
-char(4),char(0),char(96),char(0),char(46),char(0),char(5),char(0),char(27),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0),
-char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0),char(47),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),
-char(25),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(18),char(0),char(104),char(0),char(18),char(0),char(105),char(0),char(14),char(0),char(106),char(0),
-char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0),
-char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0),
+char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),
+char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),
+char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),
+char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),
+char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),
+char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97),char(99),
+char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97),char(68),
+char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114),char(68),
+char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68),char(97),
+char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97),char(0),
+char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),
+char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),
+char(8),char(0),char(0),char(0),char(12),char(0),char(36),char(0),char(8),char(0),char(16),char(0),char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),
+char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(84),char(0),char(-124),char(0),char(12),char(0),char(52),char(0),char(52),char(0),
+char(20),char(0),char(64),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),char(32),char(0),char(28),char(0),char(60),char(0),char(56),char(0),
+char(76),char(0),char(76),char(0),char(24),char(0),char(60),char(0),char(60),char(0),char(60),char(0),char(16),char(0),char(64),char(0),char(68),char(0),char(-48),char(1),
+char(0),char(1),char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-24),char(1),char(-96),char(3),char(8),char(0),char(52),char(0),char(52),char(0),
+char(0),char(0),char(68),char(0),char(84),char(0),char(-124),char(0),char(116),char(0),char(92),char(1),char(-36),char(0),char(-116),char(1),char(124),char(1),char(-44),char(0),
+char(-4),char(0),char(-52),char(1),char(92),char(1),char(116),char(2),char(124),char(2),char(-84),char(4),char(-52),char(0),char(108),char(1),char(92),char(0),char(-116),char(0),
+char(16),char(0),char(100),char(0),char(20),char(0),char(36),char(0),char(100),char(0),char(92),char(0),char(104),char(0),char(-64),char(0),char(92),char(1),char(104),char(0),
+char(-84),char(1),char(0),char(0),char(83),char(84),char(82),char(67),char(78),char(0),char(0),char(0),char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0),
+char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0),char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0),
+char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0),char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0),
+char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(15),char(0),char(1),char(0),char(13),char(0),char(9),char(0),
+char(16),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(17),char(0),char(2),char(0),char(15),char(0),char(10),char(0),char(13),char(0),char(11),char(0),
+char(18),char(0),char(2),char(0),char(16),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(19),char(0),char(4),char(0),char(4),char(0),char(12),char(0),
+char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(20),char(0),char(6),char(0),char(13),char(0),char(16),char(0),
+char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),
+char(21),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),
+char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(22),char(0),char(3),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),
+char(4),char(0),char(22),char(0),char(23),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0),char(13),char(0),char(25),char(0),
+char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(20),char(0),char(30),char(0),
+char(22),char(0),char(31),char(0),char(19),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(24),char(0),char(12),char(0),
+char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),
+char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(21),char(0),char(30),char(0),char(22),char(0),char(31),char(0),char(4),char(0),char(33),char(0),
+char(4),char(0),char(34),char(0),char(19),char(0),char(32),char(0),char(25),char(0),char(3),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(36),char(0),
+char(0),char(0),char(37),char(0),char(26),char(0),char(5),char(0),char(25),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(40),char(0),
+char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(27),char(0),char(5),char(0),char(25),char(0),char(38),char(0),char(13),char(0),char(39),char(0),
+char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(28),char(0),char(2),char(0),char(13),char(0),char(45),char(0),
+char(7),char(0),char(46),char(0),char(29),char(0),char(4),char(0),char(27),char(0),char(47),char(0),char(28),char(0),char(48),char(0),char(4),char(0),char(49),char(0),
+char(0),char(0),char(37),char(0),char(30),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(31),char(0),char(2),char(0),char(2),char(0),char(50),char(0),
+char(0),char(0),char(51),char(0),char(32),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),char(33),char(0),char(2),char(0),
+char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(34),char(0),char(8),char(0),char(13),char(0),char(54),char(0),char(14),char(0),char(55),char(0),
+char(30),char(0),char(56),char(0),char(32),char(0),char(57),char(0),char(33),char(0),char(58),char(0),char(31),char(0),char(59),char(0),char(4),char(0),char(60),char(0),
+char(4),char(0),char(61),char(0),char(35),char(0),char(4),char(0),char(34),char(0),char(62),char(0),char(13),char(0),char(63),char(0),char(4),char(0),char(64),char(0),
+char(0),char(0),char(37),char(0),char(36),char(0),char(7),char(0),char(25),char(0),char(38),char(0),char(35),char(0),char(65),char(0),char(23),char(0),char(66),char(0),
+char(24),char(0),char(67),char(0),char(37),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),char(38),char(0),char(2),char(0),
+char(36),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(39),char(0),char(4),char(0),char(17),char(0),char(71),char(0),char(25),char(0),char(72),char(0),
+char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(40),char(0),char(4),char(0),char(25),char(0),char(38),char(0),char(39),char(0),char(75),char(0),
+char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(41),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(77),char(0),
+char(0),char(0),char(37),char(0),char(42),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(78),char(0),char(0),char(0),char(37),char(0),
+char(43),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(44),char(0),char(4),char(0),
+char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0),char(37),char(0),char(14),char(0),
+char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(44),char(0),char(85),char(0),char(4),char(0),char(86),char(0),char(7),char(0),char(87),char(0),
+char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0),char(4),char(0),char(92),char(0),
+char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0),char(45),char(0),char(5),char(0),
+char(25),char(0),char(38),char(0),char(35),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(96),char(0),
+char(46),char(0),char(5),char(0),char(27),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0),char(4),char(0),char(99),char(0),
+char(0),char(0),char(100),char(0),char(47),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),char(25),char(0),char(103),char(0),
+char(0),char(0),char(35),char(0),char(18),char(0),char(104),char(0),char(18),char(0),char(105),char(0),char(14),char(0),char(106),char(0),char(14),char(0),char(107),char(0),
+char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0),char(8),char(0),char(112),char(0),
+char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0),char(4),char(0),char(117),char(0),
+char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),
+char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(48),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),
+char(25),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(17),char(0),char(104),char(0),char(17),char(0),char(105),char(0),char(13),char(0),char(106),char(0),
+char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0),char(7),char(0),char(111),char(0),
+char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0),char(7),char(0),char(116),char(0),
 char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0),
-char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(48),char(0),char(25),char(0),char(9),char(0),char(101),char(0),
-char(9),char(0),char(102),char(0),char(25),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(17),char(0),char(104),char(0),char(17),char(0),char(105),char(0),
-char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0),
-char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0),
-char(7),char(0),char(116),char(0),char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),
-char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(49),char(0),char(2),char(0),
-char(50),char(0),char(124),char(0),char(14),char(0),char(125),char(0),char(51),char(0),char(2),char(0),char(52),char(0),char(124),char(0),char(13),char(0),char(125),char(0),
-char(53),char(0),char(21),char(0),char(48),char(0),char(126),char(0),char(15),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0),
-char(13),char(0),char(-126),char(0),char(13),char(0),char(-125),char(0),char(13),char(0),char(125),char(0),char(13),char(0),char(-124),char(0),char(13),char(0),char(-123),char(0),
-char(13),char(0),char(-122),char(0),char(13),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0),
-char(7),char(0),char(-117),char(0),char(7),char(0),char(-116),char(0),char(7),char(0),char(-115),char(0),char(7),char(0),char(-114),char(0),char(7),char(0),char(-113),char(0),
-char(7),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(54),char(0),char(22),char(0),char(47),char(0),char(126),char(0),char(16),char(0),char(127),char(0),
-char(14),char(0),char(-128),char(0),char(14),char(0),char(-127),char(0),char(14),char(0),char(-126),char(0),char(14),char(0),char(-125),char(0),char(14),char(0),char(125),char(0),
-char(14),char(0),char(-124),char(0),char(14),char(0),char(-123),char(0),char(14),char(0),char(-122),char(0),char(14),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0),
-char(8),char(0),char(-119),char(0),char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(8),char(0),char(-116),char(0),char(8),char(0),char(-115),char(0),
-char(8),char(0),char(-114),char(0),char(8),char(0),char(-113),char(0),char(8),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(0),char(0),char(37),char(0),
-char(55),char(0),char(2),char(0),char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(56),char(0),char(13),char(0),char(53),char(0),char(-108),char(0),
-char(53),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),
-char(4),char(0),char(-103),char(0),char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),
-char(7),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(57),char(0),char(13),char(0),char(58),char(0),char(-108),char(0),char(58),char(0),char(-107),char(0),
+char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(49),char(0),char(2),char(0),char(50),char(0),char(124),char(0),
+char(14),char(0),char(125),char(0),char(51),char(0),char(2),char(0),char(52),char(0),char(124),char(0),char(13),char(0),char(125),char(0),char(53),char(0),char(21),char(0),
+char(48),char(0),char(126),char(0),char(15),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0),char(13),char(0),char(-126),char(0),
+char(13),char(0),char(-125),char(0),char(13),char(0),char(125),char(0),char(13),char(0),char(-124),char(0),char(13),char(0),char(-123),char(0),char(13),char(0),char(-122),char(0),
+char(13),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0),char(7),char(0),char(-117),char(0),
+char(7),char(0),char(-116),char(0),char(7),char(0),char(-115),char(0),char(7),char(0),char(-114),char(0),char(7),char(0),char(-113),char(0),char(7),char(0),char(-112),char(0),
+char(4),char(0),char(-111),char(0),char(54),char(0),char(22),char(0),char(47),char(0),char(126),char(0),char(16),char(0),char(127),char(0),char(14),char(0),char(-128),char(0),
+char(14),char(0),char(-127),char(0),char(14),char(0),char(-126),char(0),char(14),char(0),char(-125),char(0),char(14),char(0),char(125),char(0),char(14),char(0),char(-124),char(0),
+char(14),char(0),char(-123),char(0),char(14),char(0),char(-122),char(0),char(14),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0),char(8),char(0),char(-119),char(0),
+char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(8),char(0),char(-116),char(0),char(8),char(0),char(-115),char(0),char(8),char(0),char(-114),char(0),
+char(8),char(0),char(-113),char(0),char(8),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(0),char(0),char(37),char(0),char(55),char(0),char(2),char(0),
+char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(56),char(0),char(13),char(0),char(53),char(0),char(-108),char(0),char(53),char(0),char(-107),char(0),
 char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),
 char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0),
-char(4),char(0),char(-97),char(0),char(59),char(0),char(14),char(0),char(54),char(0),char(-108),char(0),char(54),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),
-char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),char(8),char(0),char(-102),char(0),
-char(8),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),
-char(0),char(0),char(-96),char(0),char(60),char(0),char(3),char(0),char(57),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0),
-char(61),char(0),char(3),char(0),char(59),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(62),char(0),char(3),char(0),
-char(57),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(63),char(0),char(13),char(0),char(57),char(0),char(-95),char(0),
-char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),
-char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),
-char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(64),char(0),char(13),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),
-char(17),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0),
+char(4),char(0),char(-97),char(0),char(57),char(0),char(13),char(0),char(58),char(0),char(-108),char(0),char(58),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),
+char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),char(7),char(0),char(-102),char(0),
+char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),
+char(59),char(0),char(14),char(0),char(54),char(0),char(-108),char(0),char(54),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),
+char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),char(8),char(0),char(-102),char(0),char(8),char(0),char(-101),char(0),
+char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(0),char(0),char(-96),char(0),
+char(60),char(0),char(3),char(0),char(57),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0),char(61),char(0),char(3),char(0),
+char(59),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(62),char(0),char(3),char(0),char(57),char(0),char(-95),char(0),
+char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(63),char(0),char(13),char(0),char(57),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),
+char(18),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0),
 char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0),
-char(7),char(0),char(-81),char(0),char(65),char(0),char(14),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),
-char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(8),char(0),char(-87),char(0),char(8),char(0),char(-86),char(0),
-char(8),char(0),char(-85),char(0),char(8),char(0),char(-84),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0),
-char(0),char(0),char(-80),char(0),char(66),char(0),char(10),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),
-char(8),char(0),char(-79),char(0),char(8),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),
-char(8),char(0),char(-81),char(0),char(8),char(0),char(-76),char(0),char(67),char(0),char(11),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),
-char(17),char(0),char(-91),char(0),char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(7),char(0),char(-83),char(0),
-char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-76),char(0),char(0),char(0),char(21),char(0),char(68),char(0),char(9),char(0),
-char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0),
-char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(69),char(0),char(9),char(0),
-char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0),
-char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(70),char(0),char(5),char(0),
-char(68),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0),
-char(71),char(0),char(5),char(0),char(69),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0),
-char(8),char(0),char(-65),char(0),char(72),char(0),char(9),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),
-char(7),char(0),char(-75),char(0),char(7),char(0),char(-74),char(0),char(7),char(0),char(-73),char(0),char(7),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),
-char(4),char(0),char(-70),char(0),char(73),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),
-char(8),char(0),char(-75),char(0),char(8),char(0),char(-74),char(0),char(8),char(0),char(-73),char(0),char(8),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),
-char(4),char(0),char(-70),char(0),char(74),char(0),char(5),char(0),char(56),char(0),char(-95),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0),
-char(7),char(0),char(-62),char(0),char(0),char(0),char(37),char(0),char(75),char(0),char(4),char(0),char(59),char(0),char(-95),char(0),char(14),char(0),char(-64),char(0),
-char(14),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(50),char(0),char(22),char(0),char(8),char(0),char(-61),char(0),char(8),char(0),char(-76),char(0),
-char(8),char(0),char(111),char(0),char(8),char(0),char(-60),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(-59),char(0),char(8),char(0),char(-58),char(0),
-char(8),char(0),char(-57),char(0),char(8),char(0),char(-56),char(0),char(8),char(0),char(-55),char(0),char(8),char(0),char(-54),char(0),char(8),char(0),char(-53),char(0),
-char(8),char(0),char(-52),char(0),char(8),char(0),char(-51),char(0),char(8),char(0),char(-50),char(0),char(8),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0),
-char(4),char(0),char(-47),char(0),char(4),char(0),char(-46),char(0),char(4),char(0),char(-45),char(0),char(4),char(0),char(-44),char(0),char(0),char(0),char(37),char(0),
-char(52),char(0),char(22),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-60),char(0),
-char(7),char(0),char(113),char(0),char(7),char(0),char(-59),char(0),char(7),char(0),char(-58),char(0),char(7),char(0),char(-57),char(0),char(7),char(0),char(-56),char(0),
-char(7),char(0),char(-55),char(0),char(7),char(0),char(-54),char(0),char(7),char(0),char(-53),char(0),char(7),char(0),char(-52),char(0),char(7),char(0),char(-51),char(0),
-char(7),char(0),char(-50),char(0),char(7),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0),char(4),char(0),char(-47),char(0),char(4),char(0),char(-46),char(0),
-char(4),char(0),char(-45),char(0),char(4),char(0),char(-44),char(0),char(0),char(0),char(37),char(0),char(76),char(0),char(4),char(0),char(7),char(0),char(-43),char(0),
-char(7),char(0),char(-42),char(0),char(7),char(0),char(-41),char(0),char(4),char(0),char(79),char(0),char(77),char(0),char(10),char(0),char(76),char(0),char(-40),char(0),
-char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(13),char(0),char(-37),char(0),char(13),char(0),char(-36),char(0),char(13),char(0),char(-35),char(0),
-char(7),char(0),char(-120),char(0),char(7),char(0),char(-34),char(0),char(4),char(0),char(-33),char(0),char(4),char(0),char(53),char(0),char(78),char(0),char(4),char(0),
-char(76),char(0),char(-40),char(0),char(4),char(0),char(-32),char(0),char(7),char(0),char(-31),char(0),char(4),char(0),char(-30),char(0),char(79),char(0),char(4),char(0),
-char(13),char(0),char(-35),char(0),char(76),char(0),char(-40),char(0),char(4),char(0),char(-29),char(0),char(7),char(0),char(-28),char(0),char(80),char(0),char(7),char(0),
-char(13),char(0),char(-27),char(0),char(76),char(0),char(-40),char(0),char(4),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0),char(7),char(0),char(-24),char(0),
-char(7),char(0),char(-23),char(0),char(4),char(0),char(53),char(0),char(81),char(0),char(6),char(0),char(15),char(0),char(-22),char(0),char(13),char(0),char(-24),char(0),
-char(13),char(0),char(-21),char(0),char(58),char(0),char(-20),char(0),char(4),char(0),char(-19),char(0),char(7),char(0),char(-23),char(0),char(82),char(0),char(26),char(0),
-char(4),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(-16),char(0),char(7),char(0),char(-15),char(0),
-char(7),char(0),char(-14),char(0),char(7),char(0),char(-13),char(0),char(7),char(0),char(-12),char(0),char(7),char(0),char(-11),char(0),char(7),char(0),char(-10),char(0),
-char(7),char(0),char(-9),char(0),char(7),char(0),char(-8),char(0),char(7),char(0),char(-7),char(0),char(7),char(0),char(-6),char(0),char(7),char(0),char(-5),char(0),
-char(7),char(0),char(-4),char(0),char(7),char(0),char(-3),char(0),char(7),char(0),char(-2),char(0),char(7),char(0),char(-1),char(0),char(7),char(0),char(0),char(1),
-char(7),char(0),char(1),char(1),char(4),char(0),char(2),char(1),char(4),char(0),char(3),char(1),char(4),char(0),char(4),char(1),char(4),char(0),char(5),char(1),
-char(4),char(0),char(118),char(0),char(83),char(0),char(12),char(0),char(15),char(0),char(6),char(1),char(15),char(0),char(7),char(1),char(15),char(0),char(8),char(1),
-char(13),char(0),char(9),char(1),char(13),char(0),char(10),char(1),char(7),char(0),char(11),char(1),char(4),char(0),char(12),char(1),char(4),char(0),char(13),char(1),
-char(4),char(0),char(14),char(1),char(4),char(0),char(15),char(1),char(7),char(0),char(-25),char(0),char(4),char(0),char(53),char(0),char(84),char(0),char(27),char(0),
-char(17),char(0),char(16),char(1),char(15),char(0),char(17),char(1),char(15),char(0),char(18),char(1),char(13),char(0),char(9),char(1),char(13),char(0),char(19),char(1),
-char(13),char(0),char(20),char(1),char(13),char(0),char(21),char(1),char(13),char(0),char(22),char(1),char(13),char(0),char(23),char(1),char(4),char(0),char(24),char(1),
-char(7),char(0),char(25),char(1),char(4),char(0),char(26),char(1),char(4),char(0),char(27),char(1),char(4),char(0),char(28),char(1),char(7),char(0),char(29),char(1),
-char(7),char(0),char(30),char(1),char(4),char(0),char(31),char(1),char(4),char(0),char(32),char(1),char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1),
-char(7),char(0),char(35),char(1),char(7),char(0),char(36),char(1),char(7),char(0),char(37),char(1),char(7),char(0),char(38),char(1),char(4),char(0),char(39),char(1),
-char(4),char(0),char(40),char(1),char(4),char(0),char(41),char(1),char(85),char(0),char(12),char(0),char(9),char(0),char(42),char(1),char(9),char(0),char(43),char(1),
-char(13),char(0),char(44),char(1),char(7),char(0),char(45),char(1),char(7),char(0),char(-57),char(0),char(7),char(0),char(46),char(1),char(4),char(0),char(47),char(1),
-char(13),char(0),char(48),char(1),char(4),char(0),char(49),char(1),char(4),char(0),char(50),char(1),char(4),char(0),char(51),char(1),char(4),char(0),char(53),char(0),
-char(86),char(0),char(19),char(0),char(48),char(0),char(126),char(0),char(83),char(0),char(52),char(1),char(76),char(0),char(53),char(1),char(77),char(0),char(54),char(1),
-char(78),char(0),char(55),char(1),char(79),char(0),char(56),char(1),char(80),char(0),char(57),char(1),char(81),char(0),char(58),char(1),char(84),char(0),char(59),char(1),
-char(85),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(4),char(0),char(27),char(1),char(4),char(0),char(62),char(1),char(4),char(0),char(63),char(1),
-char(4),char(0),char(64),char(1),char(4),char(0),char(65),char(1),char(4),char(0),char(66),char(1),char(4),char(0),char(67),char(1),char(82),char(0),char(68),char(1),
-};
+char(7),char(0),char(-81),char(0),char(64),char(0),char(13),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),
+char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),
+char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),
+char(65),char(0),char(14),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),
+char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(8),char(0),char(-87),char(0),char(8),char(0),char(-86),char(0),char(8),char(0),char(-85),char(0),
+char(8),char(0),char(-84),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0),char(0),char(0),char(-80),char(0),
+char(66),char(0),char(10),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(8),char(0),char(-79),char(0),
+char(8),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0),
+char(8),char(0),char(-76),char(0),char(67),char(0),char(11),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),
+char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0),
+char(7),char(0),char(-81),char(0),char(7),char(0),char(-76),char(0),char(0),char(0),char(21),char(0),char(68),char(0),char(9),char(0),char(57),char(0),char(-95),char(0),
+char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0),char(13),char(0),char(-73),char(0),
+char(13),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(69),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),
+char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0),char(14),char(0),char(-73),char(0),
+char(14),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(70),char(0),char(5),char(0),char(68),char(0),char(-69),char(0),
+char(4),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0),char(71),char(0),char(5),char(0),
+char(69),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0),char(8),char(0),char(-65),char(0),
+char(72),char(0),char(39),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),
+char(13),char(0),char(-74),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0),char(13),char(0),char(-62),char(0),char(13),char(0),char(-61),char(0),
+char(13),char(0),char(-60),char(0),char(13),char(0),char(-59),char(0),char(13),char(0),char(-58),char(0),char(13),char(0),char(-57),char(0),char(13),char(0),char(-56),char(0),
+char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0),char(0),char(0),char(-51),char(0),
+char(0),char(0),char(-80),char(0),char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(13),char(0),char(-50),char(0),char(13),char(0),char(-49),char(0),
+char(13),char(0),char(-48),char(0),char(13),char(0),char(-47),char(0),char(13),char(0),char(-46),char(0),char(13),char(0),char(-45),char(0),char(13),char(0),char(-44),char(0),
+char(13),char(0),char(-43),char(0),char(13),char(0),char(-42),char(0),char(13),char(0),char(-41),char(0),char(13),char(0),char(-40),char(0),char(0),char(0),char(-39),char(0),
+char(0),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),char(0),char(0),char(-36),char(0),char(4),char(0),char(-35),char(0),char(0),char(0),char(100),char(0),
+char(73),char(0),char(39),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),
+char(14),char(0),char(-74),char(0),char(14),char(0),char(-64),char(0),char(14),char(0),char(-63),char(0),char(14),char(0),char(-62),char(0),char(14),char(0),char(-61),char(0),
+char(14),char(0),char(-60),char(0),char(14),char(0),char(-59),char(0),char(14),char(0),char(-58),char(0),char(14),char(0),char(-57),char(0),char(14),char(0),char(-56),char(0),
+char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0),char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0),char(0),char(0),char(-51),char(0),
+char(0),char(0),char(-80),char(0),char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(14),char(0),char(-50),char(0),char(14),char(0),char(-49),char(0),
+char(14),char(0),char(-48),char(0),char(14),char(0),char(-47),char(0),char(14),char(0),char(-46),char(0),char(14),char(0),char(-45),char(0),char(14),char(0),char(-44),char(0),
+char(14),char(0),char(-43),char(0),char(14),char(0),char(-42),char(0),char(14),char(0),char(-41),char(0),char(14),char(0),char(-40),char(0),char(0),char(0),char(-39),char(0),
+char(0),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),char(0),char(0),char(-36),char(0),char(4),char(0),char(-35),char(0),char(0),char(0),char(100),char(0),
+char(74),char(0),char(9),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),char(7),char(0),char(-75),char(0),
+char(7),char(0),char(-74),char(0),char(7),char(0),char(-73),char(0),char(7),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),
+char(75),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(8),char(0),char(-75),char(0),
+char(8),char(0),char(-74),char(0),char(8),char(0),char(-73),char(0),char(8),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),
+char(76),char(0),char(5),char(0),char(56),char(0),char(-95),char(0),char(13),char(0),char(-34),char(0),char(13),char(0),char(-33),char(0),char(7),char(0),char(-32),char(0),
+char(0),char(0),char(37),char(0),char(77),char(0),char(4),char(0),char(59),char(0),char(-95),char(0),char(14),char(0),char(-34),char(0),char(14),char(0),char(-33),char(0),
+char(8),char(0),char(-32),char(0),char(50),char(0),char(22),char(0),char(8),char(0),char(-31),char(0),char(8),char(0),char(-76),char(0),char(8),char(0),char(111),char(0),
+char(8),char(0),char(-30),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(-29),char(0),char(8),char(0),char(-28),char(0),char(8),char(0),char(-27),char(0),
+char(8),char(0),char(-26),char(0),char(8),char(0),char(-25),char(0),char(8),char(0),char(-24),char(0),char(8),char(0),char(-23),char(0),char(8),char(0),char(-22),char(0),
+char(8),char(0),char(-21),char(0),char(8),char(0),char(-20),char(0),char(8),char(0),char(-19),char(0),char(4),char(0),char(-18),char(0),char(4),char(0),char(-17),char(0),
+char(4),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),char(0),char(0),char(37),char(0),char(52),char(0),char(22),char(0),
+char(7),char(0),char(-31),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-30),char(0),char(7),char(0),char(113),char(0),
+char(7),char(0),char(-29),char(0),char(7),char(0),char(-28),char(0),char(7),char(0),char(-27),char(0),char(7),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0),
+char(7),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-22),char(0),char(7),char(0),char(-21),char(0),char(7),char(0),char(-20),char(0),
+char(7),char(0),char(-19),char(0),char(4),char(0),char(-18),char(0),char(4),char(0),char(-17),char(0),char(4),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),
+char(4),char(0),char(-14),char(0),char(0),char(0),char(37),char(0),char(78),char(0),char(4),char(0),char(7),char(0),char(-13),char(0),char(7),char(0),char(-12),char(0),
+char(7),char(0),char(-11),char(0),char(4),char(0),char(79),char(0),char(79),char(0),char(10),char(0),char(78),char(0),char(-10),char(0),char(13),char(0),char(-9),char(0),
+char(13),char(0),char(-8),char(0),char(13),char(0),char(-7),char(0),char(13),char(0),char(-6),char(0),char(13),char(0),char(-5),char(0),char(7),char(0),char(-120),char(0),
+char(7),char(0),char(-4),char(0),char(4),char(0),char(-3),char(0),char(4),char(0),char(53),char(0),char(80),char(0),char(4),char(0),char(78),char(0),char(-10),char(0),
+char(4),char(0),char(-2),char(0),char(7),char(0),char(-1),char(0),char(4),char(0),char(0),char(1),char(81),char(0),char(4),char(0),char(13),char(0),char(-5),char(0),
+char(78),char(0),char(-10),char(0),char(4),char(0),char(1),char(1),char(7),char(0),char(2),char(1),char(82),char(0),char(7),char(0),char(13),char(0),char(3),char(1),
+char(78),char(0),char(-10),char(0),char(4),char(0),char(4),char(1),char(7),char(0),char(5),char(1),char(7),char(0),char(6),char(1),char(7),char(0),char(7),char(1),
+char(4),char(0),char(53),char(0),char(83),char(0),char(6),char(0),char(15),char(0),char(8),char(1),char(13),char(0),char(6),char(1),char(13),char(0),char(9),char(1),
+char(58),char(0),char(10),char(1),char(4),char(0),char(11),char(1),char(7),char(0),char(7),char(1),char(84),char(0),char(26),char(0),char(4),char(0),char(12),char(1),
+char(7),char(0),char(13),char(1),char(7),char(0),char(-76),char(0),char(7),char(0),char(14),char(1),char(7),char(0),char(15),char(1),char(7),char(0),char(16),char(1),
+char(7),char(0),char(17),char(1),char(7),char(0),char(18),char(1),char(7),char(0),char(19),char(1),char(7),char(0),char(20),char(1),char(7),char(0),char(21),char(1),
+char(7),char(0),char(22),char(1),char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1),char(7),char(0),char(25),char(1),char(7),char(0),char(26),char(1),
+char(7),char(0),char(27),char(1),char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1),char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1),
+char(4),char(0),char(32),char(1),char(4),char(0),char(33),char(1),char(4),char(0),char(34),char(1),char(4),char(0),char(35),char(1),char(4),char(0),char(118),char(0),
+char(85),char(0),char(12),char(0),char(15),char(0),char(36),char(1),char(15),char(0),char(37),char(1),char(15),char(0),char(38),char(1),char(13),char(0),char(39),char(1),
+char(13),char(0),char(40),char(1),char(7),char(0),char(41),char(1),char(4),char(0),char(42),char(1),char(4),char(0),char(43),char(1),char(4),char(0),char(44),char(1),
+char(4),char(0),char(45),char(1),char(7),char(0),char(5),char(1),char(4),char(0),char(53),char(0),char(86),char(0),char(27),char(0),char(17),char(0),char(46),char(1),
+char(15),char(0),char(47),char(1),char(15),char(0),char(48),char(1),char(13),char(0),char(39),char(1),char(13),char(0),char(49),char(1),char(13),char(0),char(50),char(1),
+char(13),char(0),char(51),char(1),char(13),char(0),char(52),char(1),char(13),char(0),char(53),char(1),char(4),char(0),char(54),char(1),char(7),char(0),char(55),char(1),
+char(4),char(0),char(56),char(1),char(4),char(0),char(57),char(1),char(4),char(0),char(58),char(1),char(7),char(0),char(59),char(1),char(7),char(0),char(60),char(1),
+char(4),char(0),char(61),char(1),char(4),char(0),char(62),char(1),char(7),char(0),char(63),char(1),char(7),char(0),char(64),char(1),char(7),char(0),char(65),char(1),
+char(7),char(0),char(66),char(1),char(7),char(0),char(67),char(1),char(7),char(0),char(68),char(1),char(4),char(0),char(69),char(1),char(4),char(0),char(70),char(1),
+char(4),char(0),char(71),char(1),char(87),char(0),char(12),char(0),char(9),char(0),char(72),char(1),char(9),char(0),char(73),char(1),char(13),char(0),char(74),char(1),
+char(7),char(0),char(75),char(1),char(7),char(0),char(-27),char(0),char(7),char(0),char(76),char(1),char(4),char(0),char(77),char(1),char(13),char(0),char(78),char(1),
+char(4),char(0),char(79),char(1),char(4),char(0),char(80),char(1),char(4),char(0),char(81),char(1),char(4),char(0),char(53),char(0),char(88),char(0),char(19),char(0),
+char(48),char(0),char(126),char(0),char(85),char(0),char(82),char(1),char(78),char(0),char(83),char(1),char(79),char(0),char(84),char(1),char(80),char(0),char(85),char(1),
+char(81),char(0),char(86),char(1),char(82),char(0),char(87),char(1),char(83),char(0),char(88),char(1),char(86),char(0),char(89),char(1),char(87),char(0),char(90),char(1),
+char(4),char(0),char(91),char(1),char(4),char(0),char(57),char(1),char(4),char(0),char(92),char(1),char(4),char(0),char(93),char(1),char(4),char(0),char(94),char(1),
+char(4),char(0),char(95),char(1),char(4),char(0),char(96),char(1),char(4),char(0),char(97),char(1),char(84),char(0),char(98),char(1),};
 int sBulletDNAlen= sizeof(sBulletDNAstr);
 
 char sBulletDNAstr64[]= {
-char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(69),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109),
+char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(99),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109),
 char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95),
 char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111),
 char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110),
@@ -658,334 +708,384 @@ char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char
 char(97),char(98),char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105),
 char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(83),
 char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),
-char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65),
-char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109),
-char(95),char(116),char(97),char(117),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97),
-char(120),char(69),char(114),char(114),char(111),char(114),char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111),
-char(114),char(0),char(109),char(95),char(101),char(114),char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111),
-char(98),char(97),char(108),char(67),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),
-char(101),char(80),char(101),char(110),char(101),char(116),char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),
-char(100),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110),
-char(69),char(114),char(112),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119),
-char(97),char(114),char(109),char(115),char(116),char(97),char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),
-char(109),char(97),char(120),char(71),char(121),char(114),char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109),
-char(95),char(115),char(105),char(110),char(103),char(108),char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),
-char(105),char(99),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117),
-char(109),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),
-char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99),
-char(116),char(82),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),
-char(100),char(0),char(109),char(95),char(109),char(105),char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116),
-char(99),char(104),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),
-char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),
-char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),
-char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95),
-char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),
-char(109),char(95),char(112),char(114),char(101),char(118),char(105),char(111),char(117),char(115),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),
-char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97),
-char(116),char(101),char(100),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95),
-char(97),char(114),char(101),char(97),char(0),char(109),char(95),char(97),char(116),char(116),char(97),char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101),
-char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110),
-char(103),char(116),char(104),char(0),char(109),char(95),char(98),char(98),char(101),char(110),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100),
-char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114),
-char(101),char(97),char(0),char(109),char(95),char(99),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),
-char(105),char(99),char(101),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101),
-char(0),char(109),char(95),char(99),char(49),char(0),char(109),char(95),char(99),char(50),char(0),char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111),
-char(99),char(97),char(108),char(70),char(114),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100),
-char(121),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111),
-char(77),char(111),char(100),char(101),char(108),char(0),char(109),char(95),char(98),char(97),char(117),char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95),
-char(100),char(114),char(97),char(103),char(0),char(109),char(95),char(108),char(105),char(102),char(116),char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117),
-char(114),char(101),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105),
-char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99),
-char(104),char(0),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),
-char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(107),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99),
-char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116),
-char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),
-char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),
-char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),
-char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),
-char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),
-char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),
-char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),
-char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),
-char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102),
-char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),
-char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(109),char(97),char(120),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116),
-char(105),char(109),char(101),char(83),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73),
-char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),
-char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116),
-char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116),
-char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97),
-char(108),char(101),char(0),char(109),char(95),char(97),char(113),char(113),char(0),char(109),char(95),char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111),
-char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(42),char(109),char(95),char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109),
-char(95),char(110),char(117),char(109),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87),
-char(101),char(105),char(103),char(116),char(115),char(0),char(109),char(95),char(98),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102),
-char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95),
-char(108),char(111),char(99),char(105),char(105),char(0),char(109),char(95),char(105),char(110),char(118),char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112),
-char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),
-char(91),char(50),char(93),char(0),char(109),char(95),char(108),char(118),char(0),char(109),char(95),char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97),
-char(109),char(101),char(114),char(101),char(102),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),
-char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97),
-char(109),char(101),char(82),char(101),char(102),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95),
-char(110),char(117),char(109),char(77),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109),
-char(95),char(105),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(110),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),
-char(109),char(95),char(110),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112),
-char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97),
-char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(116),char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109),
-char(97),char(120),char(83),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),
-char(115),char(101),char(0),char(109),char(95),char(115),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),
-char(112),char(117),char(108),char(115),char(101),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105),
-char(110),char(115),char(65),char(110),char(99),char(104),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109),
-char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(110),char(100),char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100),
-char(121),char(65),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50),
-char(93),char(0),char(109),char(95),char(99),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101),
-char(108),char(101),char(116),char(101),char(0),char(109),char(95),char(114),char(101),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50),
-char(93),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121),
-char(66),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109),
-char(95),char(112),char(111),char(115),char(101),char(0),char(42),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),
-char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109),
-char(95),char(102),char(97),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(116),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),
-char(0),char(42),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116),
-char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),
-char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0),
-char(109),char(95),char(110),char(117),char(109),char(70),char(97),char(99),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114),
-char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(109),char(95),char(110),char(117),char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0),
-char(109),char(95),char(110),char(117),char(109),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74),
-char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(102),char(105),char(103),char(0),char(0),char(84),char(89),char(80),char(69),
-char(87),char(0),char(0),char(0),char(99),char(104),char(97),char(114),char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),
-char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0),char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),
-char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116),char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),
-char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114),char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),
-char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101),char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),
-char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
-char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
-char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),
-char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),
-char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),
-char(97),char(116),char(97),char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),
-char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),
-char(111),char(100),char(101),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),
-char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),
-char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),
-char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),
-char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),
-char(66),char(118),char(104),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),
-char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),
-char(116),char(105),char(99),char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
-char(67),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),
-char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),
-char(105),char(117),char(115),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),
-char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),
-char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),
-char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),
-char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),
-char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),
-char(114),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),
-char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),
-char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),
-char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),
-char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),
-char(100),char(83),char(104),char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),
-char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),
-char(108),char(105),char(110),char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),
-char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108),
-char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),
-char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77),
-char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),
-char(120),char(72),char(117),char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),
-char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),
-char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),
-char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),
-char(87),char(111),char(114),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),
-char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),
-char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),
-char(100),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),
-char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),
-char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),
-char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49),
-char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),
-char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),
-char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),
-char(121),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),
-char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),
-char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),
-char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),
-char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),
-char(50),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),
-char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),
-char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),
-char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),
-char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),
-char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),
-char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),
-char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),
-char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
-char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),
-char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),
-char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),
-char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),
-char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),
-char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),
-char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(83),char(108),
-char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),
-char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),
-char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),
-char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),
+char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(66),
+char(111),char(117),char(110),char(99),char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),
+char(80),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),
+char(108),char(105),char(110),char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(108),char(105),char(110),
+char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),
+char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(108),char(105),char(110),
+char(101),char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(108),
+char(105),char(110),char(101),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(108),
+char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),
+char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),
+char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105),
+char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(110),char(97),char(98),
+char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),
+char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),
+char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(97),
+char(110),char(103),char(117),char(108),char(97),char(114),char(66),char(111),char(117),char(110),char(99),char(101),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),
+char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),
+char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(77),char(111),char(116),char(111),
+char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(67),
+char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),
+char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(77),char(97),char(120),char(77),
+char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),
+char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),
+char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),
+char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),
+char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105),char(117),char(109),char(80),
+char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),
+char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(101),
+char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),
+char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(112),
+char(97),char(100),char(100),char(105),char(110),char(103),char(50),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(111),char(116),char(97),char(116),char(101),char(79),
+char(114),char(100),char(101),char(114),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65),char(0),char(109),char(95),char(97),char(120),char(105),
+char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109),char(95),char(116),char(97),char(117),char(0),char(109),
+char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97),char(120),char(69),char(114),char(114),char(111),char(114),
+char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111),char(114),char(0),char(109),char(95),char(101),char(114),
+char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111),char(98),char(97),char(108),char(67),char(102),char(109),
+char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(80),char(101),char(110),char(101),char(116),
+char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(115),char(112),
+char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110),char(69),char(114),char(112),char(0),char(109),char(95),
+char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119),char(97),char(114),char(109),char(115),char(116),char(97),
+char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(97),char(120),char(71),char(121),char(114),
+char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(115),char(105),char(110),char(103),char(108),
+char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),
+char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117),char(109),char(73),char(116),char(101),char(114),char(97),
+char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(77),char(111),char(100),char(101),char(0),char(109),
+char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(82),char(101),char(115),char(116),char(105),
+char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(109),char(105),
+char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116),char(99),char(104),char(83),char(105),char(122),char(101),
+char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(105),
+char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117),
+char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109),
+char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),
+char(97),char(108),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(114),char(101),char(118),
+char(105),char(111),char(117),char(115),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),
+char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97),char(116),char(101),char(100),char(70),char(111),char(114),
+char(99),char(101),char(0),char(109),char(95),char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95),char(97),char(114),char(101),char(97),char(0),char(109),
+char(95),char(97),char(116),char(116),char(97),char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),
+char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110),char(103),char(116),char(104),char(0),char(109),char(95),
+char(98),char(98),char(101),char(110),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),
+char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114),char(101),char(97),char(0),char(109),char(95),char(99),
+char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(52),
+char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(99),char(49),char(0),
+char(109),char(95),char(99),char(50),char(0),char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(70),char(114),char(97),
+char(109),char(101),char(0),char(42),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(0),char(109),char(95),char(110),char(111),
+char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111),char(77),char(111),char(100),char(101),char(108),char(0),
+char(109),char(95),char(98),char(97),char(117),char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95),char(100),char(114),char(97),char(103),char(0),char(109),
+char(95),char(108),char(105),char(102),char(116),char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117),char(114),char(101),char(0),char(109),char(95),char(118),
+char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105),char(99),char(70),char(114),char(105),char(99),char(116),
+char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99),char(104),char(0),char(109),char(95),char(114),char(105),
+char(103),char(105),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),
+char(95),char(107),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),
+char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),
+char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(72),char(97),char(114),char(100),char(110),char(101),
+char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101),
+char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101),
+char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),
+char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),
+char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115),
+char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),
+char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),
+char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),
+char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),
+char(95),char(109),char(97),char(120),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(99),char(97),
+char(108),char(101),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73),char(116),char(101),char(114),char(97),char(116),char(105),
+char(111),char(110),char(115),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(73),char(116),char(101),char(114),char(97),char(116),
+char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),
+char(110),char(115),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),
+char(110),char(115),char(0),char(109),char(95),char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(97),
+char(113),char(113),char(0),char(109),char(95),char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),
+char(115),char(0),char(42),char(109),char(95),char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(80),char(111),
+char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87),char(101),char(105),char(103),char(116),char(115),char(0),
+char(109),char(95),char(98),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102),char(114),char(97),char(109),char(101),char(0),char(109),
+char(95),char(102),char(114),char(97),char(109),char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(108),char(111),char(99),char(105),char(105),char(0),
+char(109),char(95),char(105),char(110),char(118),char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91),
+char(50),char(93),char(0),char(109),char(95),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),
+char(108),char(118),char(0),char(109),char(95),char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(114),char(101),char(102),char(115),
+char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109),
+char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97),char(109),char(101),char(82),char(101),char(102),char(115),
+char(0),char(109),char(95),char(110),char(117),char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(115),
+char(115),char(101),char(115),char(0),char(109),char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(105),char(109),char(97),char(115),char(115),
+char(0),char(109),char(95),char(110),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(105),char(109),
+char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),
+char(108),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),
+char(109),char(95),char(109),char(97),char(116),char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(120),char(83),char(101),char(108),char(102),
+char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(115),
+char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(70),
+char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105),char(110),char(115),char(65),char(110),char(99),char(104),
+char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),
+char(101),char(114),char(73),char(110),char(100),char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(0),char(42),char(109),char(95),
+char(98),char(111),char(100),char(121),char(66),char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(99),char(102),
+char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101),char(108),char(101),char(116),char(101),char(0),char(109),
+char(95),char(114),char(101),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50),char(93),char(0),char(109),char(95),char(98),char(111),
+char(100),char(121),char(65),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(116),char(121),char(112),char(101),char(0),
+char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(101),char(0),
+char(42),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100),
+char(101),char(115),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109),char(95),char(102),char(97),char(99),char(101),char(115),
+char(0),char(42),char(109),char(95),char(116),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(42),char(109),char(95),char(97),char(110),
+char(99),char(104),char(111),char(114),char(115),char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(42),char(109),
+char(95),char(106),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(116),char(101),char(114),char(105),char(97),
+char(108),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),
+char(97),char(99),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),
+char(0),char(109),char(95),char(110),char(117),char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(67),
+char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74),char(111),char(105),char(110),char(116),char(115),char(0),
+char(109),char(95),char(99),char(111),char(110),char(102),char(105),char(103),char(0),char(0),char(0),char(0),char(84),char(89),char(80),char(69),char(89),char(0),char(0),char(0),
+char(99),char(104),char(97),char(114),char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),char(0),char(117),char(115),char(104),
+char(111),char(114),char(116),char(0),char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),char(110),char(103),char(0),char(102),
+char(108),char(111),char(97),char(116),char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),char(0),char(80),char(111),char(105),
+char(110),char(116),char(101),char(114),char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),char(105),char(99),char(115),char(83),
+char(121),char(115),char(116),char(101),char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),char(116),char(86),char(101),char(99),
+char(116),char(111),char(114),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(86),char(101),char(99),char(116),
+char(111),char(114),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114),
+char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),
+char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),
+char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),
+char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(70),
+char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),
+char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),
+char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97),char(116),
+char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(68),
+char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),
+char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99),char(80),
+char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),
+char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115),char(0),
+char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),
+char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116),
+char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),
+char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68),char(97),
+char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110),char(116),
+char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),
+char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),
+char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),
+char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),
+char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),
+char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),
+char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110),char(100),
+char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83),char(104),
+char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104),char(97),
+char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),
+char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104),char(83),
+char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117),char(108),
+char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),
+char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),
+char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111),char(97),
+char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),
+char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),
+char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),
+char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70),char(108),char(111),
+char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),
+char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),
+char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),
+char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),
+char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49),char(0),char(98),char(116),char(84),
+char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),
+char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),
+char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97),char(116),
+char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),
+char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),
+char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),
+char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),
+char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),
+char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),
+char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),
+char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),
+char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),
+char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(67),char(111),
+char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),
+char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),
+char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),
+char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),
+char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),
+char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),
+char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),
+char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),
+char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),
+char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),
+char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),
+char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),
+char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),
+char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),
+char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),
 char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),
-char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116),
-char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83),
-char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),
-char(66),char(111),char(100),char(121),char(70),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),
-char(121),char(84),char(101),char(116),char(114),char(97),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),
-char(65),char(110),char(99),char(104),char(111),char(114),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),
-char(111),char(110),char(102),char(105),char(103),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111),
-char(115),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116),
-char(101),char(114),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105),
-char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111),
-char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),
-char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0),char(16),char(0),char(48),char(0),char(16),char(0),char(16),char(0),
-char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(96),char(0),
-char(-112),char(0),char(16),char(0),char(56),char(0),char(56),char(0),char(20),char(0),char(72),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),
-char(56),char(0),char(32),char(0),char(80),char(0),char(72),char(0),char(96),char(0),char(80),char(0),char(32),char(0),char(64),char(0),char(64),char(0),char(64),char(0),
-char(16),char(0),char(72),char(0),char(80),char(0),char(-32),char(1),char(16),char(1),char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-8),char(1),
-char(-80),char(3),char(8),char(0),char(64),char(0),char(64),char(0),char(0),char(0),char(80),char(0),char(96),char(0),char(-112),char(0),char(-128),char(0),char(104),char(1),
-char(-24),char(0),char(-104),char(1),char(-120),char(1),char(-32),char(0),char(8),char(1),char(-40),char(1),char(104),char(1),char(-128),char(2),char(-40),char(0),char(120),char(1),
-char(104),char(0),char(-104),char(0),char(16),char(0),char(104),char(0),char(24),char(0),char(40),char(0),char(104),char(0),char(96),char(0),char(104),char(0),char(-56),char(0),
-char(104),char(1),char(112),char(0),char(-32),char(1),char(0),char(0),char(83),char(84),char(82),char(67),char(76),char(0),char(0),char(0),char(10),char(0),char(3),char(0),
-char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0),char(10),char(0),char(3),char(0),
-char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0),char(9),char(0),char(7),char(0),
-char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(15),char(0),char(1),char(0),
-char(13),char(0),char(9),char(0),char(16),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(17),char(0),char(2),char(0),char(15),char(0),char(10),char(0),
-char(13),char(0),char(11),char(0),char(18),char(0),char(2),char(0),char(16),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(19),char(0),char(4),char(0),
-char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(20),char(0),char(6),char(0),
-char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),
-char(0),char(0),char(21),char(0),char(21),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0),
-char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(22),char(0),char(3),char(0),char(2),char(0),char(14),char(0),
-char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(23),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0),
-char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),
-char(20),char(0),char(30),char(0),char(22),char(0),char(31),char(0),char(19),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),
-char(24),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0),
-char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(21),char(0),char(30),char(0),char(22),char(0),char(31),char(0),
-char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(19),char(0),char(32),char(0),char(25),char(0),char(3),char(0),char(0),char(0),char(35),char(0),
-char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(26),char(0),char(5),char(0),char(25),char(0),char(38),char(0),char(13),char(0),char(39),char(0),
-char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(27),char(0),char(5),char(0),char(25),char(0),char(38),char(0),
-char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(28),char(0),char(2),char(0),
-char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(29),char(0),char(4),char(0),char(27),char(0),char(47),char(0),char(28),char(0),char(48),char(0),
-char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(30),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(31),char(0),char(2),char(0),
-char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(32),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),
-char(33),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(34),char(0),char(8),char(0),char(13),char(0),char(54),char(0),
-char(14),char(0),char(55),char(0),char(30),char(0),char(56),char(0),char(32),char(0),char(57),char(0),char(33),char(0),char(58),char(0),char(31),char(0),char(59),char(0),
-char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(35),char(0),char(4),char(0),char(34),char(0),char(62),char(0),char(13),char(0),char(63),char(0),
-char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(36),char(0),char(7),char(0),char(25),char(0),char(38),char(0),char(35),char(0),char(65),char(0),
-char(23),char(0),char(66),char(0),char(24),char(0),char(67),char(0),char(37),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),
-char(38),char(0),char(2),char(0),char(36),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(39),char(0),char(4),char(0),char(17),char(0),char(71),char(0),
-char(25),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(40),char(0),char(4),char(0),char(25),char(0),char(38),char(0),
-char(39),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(41),char(0),char(3),char(0),char(27),char(0),char(47),char(0),
-char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(42),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(78),char(0),
-char(0),char(0),char(37),char(0),char(43),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),
-char(44),char(0),char(4),char(0),char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0),
-char(37),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(44),char(0),char(85),char(0),char(4),char(0),char(86),char(0),
-char(7),char(0),char(87),char(0),char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0),
-char(4),char(0),char(92),char(0),char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0),
-char(45),char(0),char(5),char(0),char(25),char(0),char(38),char(0),char(35),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0),
-char(4),char(0),char(96),char(0),char(46),char(0),char(5),char(0),char(27),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0),
-char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0),char(47),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),
-char(25),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(18),char(0),char(104),char(0),char(18),char(0),char(105),char(0),char(14),char(0),char(106),char(0),
-char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0),
-char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0),
+char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),
+char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),
+char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),
+char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),
+char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),
+char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97),char(99),
+char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97),char(68),
+char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114),char(68),
+char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68),char(97),
+char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97),char(0),
+char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),
+char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),
+char(8),char(0),char(0),char(0),char(16),char(0),char(48),char(0),char(16),char(0),char(16),char(0),char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),
+char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(96),char(0),char(-112),char(0),char(16),char(0),char(56),char(0),char(56),char(0),
+char(20),char(0),char(72),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),char(56),char(0),char(32),char(0),char(80),char(0),char(72),char(0),
+char(96),char(0),char(80),char(0),char(32),char(0),char(64),char(0),char(64),char(0),char(64),char(0),char(16),char(0),char(72),char(0),char(80),char(0),char(-32),char(1),
+char(16),char(1),char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-8),char(1),char(-80),char(3),char(8),char(0),char(64),char(0),char(64),char(0),
+char(0),char(0),char(80),char(0),char(96),char(0),char(-112),char(0),char(-128),char(0),char(104),char(1),char(-24),char(0),char(-104),char(1),char(-120),char(1),char(-32),char(0),
+char(8),char(1),char(-40),char(1),char(104),char(1),char(-128),char(2),char(-120),char(2),char(-72),char(4),char(-40),char(0),char(120),char(1),char(104),char(0),char(-104),char(0),
+char(16),char(0),char(104),char(0),char(24),char(0),char(40),char(0),char(104),char(0),char(96),char(0),char(104),char(0),char(-56),char(0),char(104),char(1),char(112),char(0),
+char(-32),char(1),char(0),char(0),char(83),char(84),char(82),char(67),char(78),char(0),char(0),char(0),char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0),
+char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0),char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0),
+char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0),char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0),
+char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(15),char(0),char(1),char(0),char(13),char(0),char(9),char(0),
+char(16),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(17),char(0),char(2),char(0),char(15),char(0),char(10),char(0),char(13),char(0),char(11),char(0),
+char(18),char(0),char(2),char(0),char(16),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(19),char(0),char(4),char(0),char(4),char(0),char(12),char(0),
+char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(20),char(0),char(6),char(0),char(13),char(0),char(16),char(0),
+char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),
+char(21),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),
+char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(22),char(0),char(3),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),
+char(4),char(0),char(22),char(0),char(23),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0),char(13),char(0),char(25),char(0),
+char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(20),char(0),char(30),char(0),
+char(22),char(0),char(31),char(0),char(19),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(24),char(0),char(12),char(0),
+char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),
+char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(21),char(0),char(30),char(0),char(22),char(0),char(31),char(0),char(4),char(0),char(33),char(0),
+char(4),char(0),char(34),char(0),char(19),char(0),char(32),char(0),char(25),char(0),char(3),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(36),char(0),
+char(0),char(0),char(37),char(0),char(26),char(0),char(5),char(0),char(25),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(40),char(0),
+char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(27),char(0),char(5),char(0),char(25),char(0),char(38),char(0),char(13),char(0),char(39),char(0),
+char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(28),char(0),char(2),char(0),char(13),char(0),char(45),char(0),
+char(7),char(0),char(46),char(0),char(29),char(0),char(4),char(0),char(27),char(0),char(47),char(0),char(28),char(0),char(48),char(0),char(4),char(0),char(49),char(0),
+char(0),char(0),char(37),char(0),char(30),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(31),char(0),char(2),char(0),char(2),char(0),char(50),char(0),
+char(0),char(0),char(51),char(0),char(32),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),char(33),char(0),char(2),char(0),
+char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(34),char(0),char(8),char(0),char(13),char(0),char(54),char(0),char(14),char(0),char(55),char(0),
+char(30),char(0),char(56),char(0),char(32),char(0),char(57),char(0),char(33),char(0),char(58),char(0),char(31),char(0),char(59),char(0),char(4),char(0),char(60),char(0),
+char(4),char(0),char(61),char(0),char(35),char(0),char(4),char(0),char(34),char(0),char(62),char(0),char(13),char(0),char(63),char(0),char(4),char(0),char(64),char(0),
+char(0),char(0),char(37),char(0),char(36),char(0),char(7),char(0),char(25),char(0),char(38),char(0),char(35),char(0),char(65),char(0),char(23),char(0),char(66),char(0),
+char(24),char(0),char(67),char(0),char(37),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),char(38),char(0),char(2),char(0),
+char(36),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(39),char(0),char(4),char(0),char(17),char(0),char(71),char(0),char(25),char(0),char(72),char(0),
+char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(40),char(0),char(4),char(0),char(25),char(0),char(38),char(0),char(39),char(0),char(75),char(0),
+char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(41),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(77),char(0),
+char(0),char(0),char(37),char(0),char(42),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(78),char(0),char(0),char(0),char(37),char(0),
+char(43),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(44),char(0),char(4),char(0),
+char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0),char(37),char(0),char(14),char(0),
+char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(44),char(0),char(85),char(0),char(4),char(0),char(86),char(0),char(7),char(0),char(87),char(0),
+char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0),char(4),char(0),char(92),char(0),
+char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0),char(45),char(0),char(5),char(0),
+char(25),char(0),char(38),char(0),char(35),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(96),char(0),
+char(46),char(0),char(5),char(0),char(27),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0),char(4),char(0),char(99),char(0),
+char(0),char(0),char(100),char(0),char(47),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),char(25),char(0),char(103),char(0),
+char(0),char(0),char(35),char(0),char(18),char(0),char(104),char(0),char(18),char(0),char(105),char(0),char(14),char(0),char(106),char(0),char(14),char(0),char(107),char(0),
+char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0),char(8),char(0),char(112),char(0),
+char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0),char(4),char(0),char(117),char(0),
+char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),
+char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(48),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),
+char(25),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(17),char(0),char(104),char(0),char(17),char(0),char(105),char(0),char(13),char(0),char(106),char(0),
+char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0),char(7),char(0),char(111),char(0),
+char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0),char(7),char(0),char(116),char(0),
 char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0),
-char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(48),char(0),char(25),char(0),char(9),char(0),char(101),char(0),
-char(9),char(0),char(102),char(0),char(25),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(17),char(0),char(104),char(0),char(17),char(0),char(105),char(0),
-char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0),
-char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0),
-char(7),char(0),char(116),char(0),char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),
-char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(49),char(0),char(2),char(0),
-char(50),char(0),char(124),char(0),char(14),char(0),char(125),char(0),char(51),char(0),char(2),char(0),char(52),char(0),char(124),char(0),char(13),char(0),char(125),char(0),
-char(53),char(0),char(21),char(0),char(48),char(0),char(126),char(0),char(15),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0),
-char(13),char(0),char(-126),char(0),char(13),char(0),char(-125),char(0),char(13),char(0),char(125),char(0),char(13),char(0),char(-124),char(0),char(13),char(0),char(-123),char(0),
-char(13),char(0),char(-122),char(0),char(13),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0),
-char(7),char(0),char(-117),char(0),char(7),char(0),char(-116),char(0),char(7),char(0),char(-115),char(0),char(7),char(0),char(-114),char(0),char(7),char(0),char(-113),char(0),
-char(7),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(54),char(0),char(22),char(0),char(47),char(0),char(126),char(0),char(16),char(0),char(127),char(0),
-char(14),char(0),char(-128),char(0),char(14),char(0),char(-127),char(0),char(14),char(0),char(-126),char(0),char(14),char(0),char(-125),char(0),char(14),char(0),char(125),char(0),
-char(14),char(0),char(-124),char(0),char(14),char(0),char(-123),char(0),char(14),char(0),char(-122),char(0),char(14),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0),
-char(8),char(0),char(-119),char(0),char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(8),char(0),char(-116),char(0),char(8),char(0),char(-115),char(0),
-char(8),char(0),char(-114),char(0),char(8),char(0),char(-113),char(0),char(8),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(0),char(0),char(37),char(0),
-char(55),char(0),char(2),char(0),char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(56),char(0),char(13),char(0),char(53),char(0),char(-108),char(0),
-char(53),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),
-char(4),char(0),char(-103),char(0),char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),
-char(7),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(57),char(0),char(13),char(0),char(58),char(0),char(-108),char(0),char(58),char(0),char(-107),char(0),
+char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(49),char(0),char(2),char(0),char(50),char(0),char(124),char(0),
+char(14),char(0),char(125),char(0),char(51),char(0),char(2),char(0),char(52),char(0),char(124),char(0),char(13),char(0),char(125),char(0),char(53),char(0),char(21),char(0),
+char(48),char(0),char(126),char(0),char(15),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0),char(13),char(0),char(-126),char(0),
+char(13),char(0),char(-125),char(0),char(13),char(0),char(125),char(0),char(13),char(0),char(-124),char(0),char(13),char(0),char(-123),char(0),char(13),char(0),char(-122),char(0),
+char(13),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0),char(7),char(0),char(-117),char(0),
+char(7),char(0),char(-116),char(0),char(7),char(0),char(-115),char(0),char(7),char(0),char(-114),char(0),char(7),char(0),char(-113),char(0),char(7),char(0),char(-112),char(0),
+char(4),char(0),char(-111),char(0),char(54),char(0),char(22),char(0),char(47),char(0),char(126),char(0),char(16),char(0),char(127),char(0),char(14),char(0),char(-128),char(0),
+char(14),char(0),char(-127),char(0),char(14),char(0),char(-126),char(0),char(14),char(0),char(-125),char(0),char(14),char(0),char(125),char(0),char(14),char(0),char(-124),char(0),
+char(14),char(0),char(-123),char(0),char(14),char(0),char(-122),char(0),char(14),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0),char(8),char(0),char(-119),char(0),
+char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(8),char(0),char(-116),char(0),char(8),char(0),char(-115),char(0),char(8),char(0),char(-114),char(0),
+char(8),char(0),char(-113),char(0),char(8),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(0),char(0),char(37),char(0),char(55),char(0),char(2),char(0),
+char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(56),char(0),char(13),char(0),char(53),char(0),char(-108),char(0),char(53),char(0),char(-107),char(0),
 char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),
 char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0),
-char(4),char(0),char(-97),char(0),char(59),char(0),char(14),char(0),char(54),char(0),char(-108),char(0),char(54),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),
-char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),char(8),char(0),char(-102),char(0),
-char(8),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),
-char(0),char(0),char(-96),char(0),char(60),char(0),char(3),char(0),char(57),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0),
-char(61),char(0),char(3),char(0),char(59),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(62),char(0),char(3),char(0),
-char(57),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(63),char(0),char(13),char(0),char(57),char(0),char(-95),char(0),
-char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),
-char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),
-char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(64),char(0),char(13),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),
-char(17),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0),
+char(4),char(0),char(-97),char(0),char(57),char(0),char(13),char(0),char(58),char(0),char(-108),char(0),char(58),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),
+char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),char(7),char(0),char(-102),char(0),
+char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),
+char(59),char(0),char(14),char(0),char(54),char(0),char(-108),char(0),char(54),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),
+char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),char(8),char(0),char(-102),char(0),char(8),char(0),char(-101),char(0),
+char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(0),char(0),char(-96),char(0),
+char(60),char(0),char(3),char(0),char(57),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0),char(61),char(0),char(3),char(0),
+char(59),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(62),char(0),char(3),char(0),char(57),char(0),char(-95),char(0),
+char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(63),char(0),char(13),char(0),char(57),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),
+char(18),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0),
 char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0),
-char(7),char(0),char(-81),char(0),char(65),char(0),char(14),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),
-char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(8),char(0),char(-87),char(0),char(8),char(0),char(-86),char(0),
-char(8),char(0),char(-85),char(0),char(8),char(0),char(-84),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0),
-char(0),char(0),char(-80),char(0),char(66),char(0),char(10),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),
-char(8),char(0),char(-79),char(0),char(8),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),
-char(8),char(0),char(-81),char(0),char(8),char(0),char(-76),char(0),char(67),char(0),char(11),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),
-char(17),char(0),char(-91),char(0),char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(7),char(0),char(-83),char(0),
-char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-76),char(0),char(0),char(0),char(21),char(0),char(68),char(0),char(9),char(0),
-char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0),
-char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(69),char(0),char(9),char(0),
-char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0),
-char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(70),char(0),char(5),char(0),
-char(68),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0),
-char(71),char(0),char(5),char(0),char(69),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0),
-char(8),char(0),char(-65),char(0),char(72),char(0),char(9),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),
-char(7),char(0),char(-75),char(0),char(7),char(0),char(-74),char(0),char(7),char(0),char(-73),char(0),char(7),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),
-char(4),char(0),char(-70),char(0),char(73),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),
-char(8),char(0),char(-75),char(0),char(8),char(0),char(-74),char(0),char(8),char(0),char(-73),char(0),char(8),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),
-char(4),char(0),char(-70),char(0),char(74),char(0),char(5),char(0),char(56),char(0),char(-95),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0),
-char(7),char(0),char(-62),char(0),char(0),char(0),char(37),char(0),char(75),char(0),char(4),char(0),char(59),char(0),char(-95),char(0),char(14),char(0),char(-64),char(0),
-char(14),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(50),char(0),char(22),char(0),char(8),char(0),char(-61),char(0),char(8),char(0),char(-76),char(0),
-char(8),char(0),char(111),char(0),char(8),char(0),char(-60),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(-59),char(0),char(8),char(0),char(-58),char(0),
-char(8),char(0),char(-57),char(0),char(8),char(0),char(-56),char(0),char(8),char(0),char(-55),char(0),char(8),char(0),char(-54),char(0),char(8),char(0),char(-53),char(0),
-char(8),char(0),char(-52),char(0),char(8),char(0),char(-51),char(0),char(8),char(0),char(-50),char(0),char(8),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0),
-char(4),char(0),char(-47),char(0),char(4),char(0),char(-46),char(0),char(4),char(0),char(-45),char(0),char(4),char(0),char(-44),char(0),char(0),char(0),char(37),char(0),
-char(52),char(0),char(22),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-60),char(0),
-char(7),char(0),char(113),char(0),char(7),char(0),char(-59),char(0),char(7),char(0),char(-58),char(0),char(7),char(0),char(-57),char(0),char(7),char(0),char(-56),char(0),
-char(7),char(0),char(-55),char(0),char(7),char(0),char(-54),char(0),char(7),char(0),char(-53),char(0),char(7),char(0),char(-52),char(0),char(7),char(0),char(-51),char(0),
-char(7),char(0),char(-50),char(0),char(7),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0),char(4),char(0),char(-47),char(0),char(4),char(0),char(-46),char(0),
-char(4),char(0),char(-45),char(0),char(4),char(0),char(-44),char(0),char(0),char(0),char(37),char(0),char(76),char(0),char(4),char(0),char(7),char(0),char(-43),char(0),
-char(7),char(0),char(-42),char(0),char(7),char(0),char(-41),char(0),char(4),char(0),char(79),char(0),char(77),char(0),char(10),char(0),char(76),char(0),char(-40),char(0),
-char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(13),char(0),char(-37),char(0),char(13),char(0),char(-36),char(0),char(13),char(0),char(-35),char(0),
-char(7),char(0),char(-120),char(0),char(7),char(0),char(-34),char(0),char(4),char(0),char(-33),char(0),char(4),char(0),char(53),char(0),char(78),char(0),char(4),char(0),
-char(76),char(0),char(-40),char(0),char(4),char(0),char(-32),char(0),char(7),char(0),char(-31),char(0),char(4),char(0),char(-30),char(0),char(79),char(0),char(4),char(0),
-char(13),char(0),char(-35),char(0),char(76),char(0),char(-40),char(0),char(4),char(0),char(-29),char(0),char(7),char(0),char(-28),char(0),char(80),char(0),char(7),char(0),
-char(13),char(0),char(-27),char(0),char(76),char(0),char(-40),char(0),char(4),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0),char(7),char(0),char(-24),char(0),
-char(7),char(0),char(-23),char(0),char(4),char(0),char(53),char(0),char(81),char(0),char(6),char(0),char(15),char(0),char(-22),char(0),char(13),char(0),char(-24),char(0),
-char(13),char(0),char(-21),char(0),char(58),char(0),char(-20),char(0),char(4),char(0),char(-19),char(0),char(7),char(0),char(-23),char(0),char(82),char(0),char(26),char(0),
-char(4),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(-16),char(0),char(7),char(0),char(-15),char(0),
-char(7),char(0),char(-14),char(0),char(7),char(0),char(-13),char(0),char(7),char(0),char(-12),char(0),char(7),char(0),char(-11),char(0),char(7),char(0),char(-10),char(0),
-char(7),char(0),char(-9),char(0),char(7),char(0),char(-8),char(0),char(7),char(0),char(-7),char(0),char(7),char(0),char(-6),char(0),char(7),char(0),char(-5),char(0),
-char(7),char(0),char(-4),char(0),char(7),char(0),char(-3),char(0),char(7),char(0),char(-2),char(0),char(7),char(0),char(-1),char(0),char(7),char(0),char(0),char(1),
-char(7),char(0),char(1),char(1),char(4),char(0),char(2),char(1),char(4),char(0),char(3),char(1),char(4),char(0),char(4),char(1),char(4),char(0),char(5),char(1),
-char(4),char(0),char(118),char(0),char(83),char(0),char(12),char(0),char(15),char(0),char(6),char(1),char(15),char(0),char(7),char(1),char(15),char(0),char(8),char(1),
-char(13),char(0),char(9),char(1),char(13),char(0),char(10),char(1),char(7),char(0),char(11),char(1),char(4),char(0),char(12),char(1),char(4),char(0),char(13),char(1),
-char(4),char(0),char(14),char(1),char(4),char(0),char(15),char(1),char(7),char(0),char(-25),char(0),char(4),char(0),char(53),char(0),char(84),char(0),char(27),char(0),
-char(17),char(0),char(16),char(1),char(15),char(0),char(17),char(1),char(15),char(0),char(18),char(1),char(13),char(0),char(9),char(1),char(13),char(0),char(19),char(1),
-char(13),char(0),char(20),char(1),char(13),char(0),char(21),char(1),char(13),char(0),char(22),char(1),char(13),char(0),char(23),char(1),char(4),char(0),char(24),char(1),
-char(7),char(0),char(25),char(1),char(4),char(0),char(26),char(1),char(4),char(0),char(27),char(1),char(4),char(0),char(28),char(1),char(7),char(0),char(29),char(1),
-char(7),char(0),char(30),char(1),char(4),char(0),char(31),char(1),char(4),char(0),char(32),char(1),char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1),
-char(7),char(0),char(35),char(1),char(7),char(0),char(36),char(1),char(7),char(0),char(37),char(1),char(7),char(0),char(38),char(1),char(4),char(0),char(39),char(1),
-char(4),char(0),char(40),char(1),char(4),char(0),char(41),char(1),char(85),char(0),char(12),char(0),char(9),char(0),char(42),char(1),char(9),char(0),char(43),char(1),
-char(13),char(0),char(44),char(1),char(7),char(0),char(45),char(1),char(7),char(0),char(-57),char(0),char(7),char(0),char(46),char(1),char(4),char(0),char(47),char(1),
-char(13),char(0),char(48),char(1),char(4),char(0),char(49),char(1),char(4),char(0),char(50),char(1),char(4),char(0),char(51),char(1),char(4),char(0),char(53),char(0),
-char(86),char(0),char(19),char(0),char(48),char(0),char(126),char(0),char(83),char(0),char(52),char(1),char(76),char(0),char(53),char(1),char(77),char(0),char(54),char(1),
-char(78),char(0),char(55),char(1),char(79),char(0),char(56),char(1),char(80),char(0),char(57),char(1),char(81),char(0),char(58),char(1),char(84),char(0),char(59),char(1),
-char(85),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(4),char(0),char(27),char(1),char(4),char(0),char(62),char(1),char(4),char(0),char(63),char(1),
-char(4),char(0),char(64),char(1),char(4),char(0),char(65),char(1),char(4),char(0),char(66),char(1),char(4),char(0),char(67),char(1),char(82),char(0),char(68),char(1),
-};
+char(7),char(0),char(-81),char(0),char(64),char(0),char(13),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),
+char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),
+char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),
+char(65),char(0),char(14),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),
+char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(8),char(0),char(-87),char(0),char(8),char(0),char(-86),char(0),char(8),char(0),char(-85),char(0),
+char(8),char(0),char(-84),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0),char(0),char(0),char(-80),char(0),
+char(66),char(0),char(10),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(8),char(0),char(-79),char(0),
+char(8),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0),
+char(8),char(0),char(-76),char(0),char(67),char(0),char(11),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),
+char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0),
+char(7),char(0),char(-81),char(0),char(7),char(0),char(-76),char(0),char(0),char(0),char(21),char(0),char(68),char(0),char(9),char(0),char(57),char(0),char(-95),char(0),
+char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0),char(13),char(0),char(-73),char(0),
+char(13),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(69),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),
+char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0),char(14),char(0),char(-73),char(0),
+char(14),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(70),char(0),char(5),char(0),char(68),char(0),char(-69),char(0),
+char(4),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0),char(71),char(0),char(5),char(0),
+char(69),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0),char(8),char(0),char(-65),char(0),
+char(72),char(0),char(39),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),
+char(13),char(0),char(-74),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0),char(13),char(0),char(-62),char(0),char(13),char(0),char(-61),char(0),
+char(13),char(0),char(-60),char(0),char(13),char(0),char(-59),char(0),char(13),char(0),char(-58),char(0),char(13),char(0),char(-57),char(0),char(13),char(0),char(-56),char(0),
+char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0),char(0),char(0),char(-51),char(0),
+char(0),char(0),char(-80),char(0),char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(13),char(0),char(-50),char(0),char(13),char(0),char(-49),char(0),
+char(13),char(0),char(-48),char(0),char(13),char(0),char(-47),char(0),char(13),char(0),char(-46),char(0),char(13),char(0),char(-45),char(0),char(13),char(0),char(-44),char(0),
+char(13),char(0),char(-43),char(0),char(13),char(0),char(-42),char(0),char(13),char(0),char(-41),char(0),char(13),char(0),char(-40),char(0),char(0),char(0),char(-39),char(0),
+char(0),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),char(0),char(0),char(-36),char(0),char(4),char(0),char(-35),char(0),char(0),char(0),char(100),char(0),
+char(73),char(0),char(39),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),
+char(14),char(0),char(-74),char(0),char(14),char(0),char(-64),char(0),char(14),char(0),char(-63),char(0),char(14),char(0),char(-62),char(0),char(14),char(0),char(-61),char(0),
+char(14),char(0),char(-60),char(0),char(14),char(0),char(-59),char(0),char(14),char(0),char(-58),char(0),char(14),char(0),char(-57),char(0),char(14),char(0),char(-56),char(0),
+char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0),char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0),char(0),char(0),char(-51),char(0),
+char(0),char(0),char(-80),char(0),char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(14),char(0),char(-50),char(0),char(14),char(0),char(-49),char(0),
+char(14),char(0),char(-48),char(0),char(14),char(0),char(-47),char(0),char(14),char(0),char(-46),char(0),char(14),char(0),char(-45),char(0),char(14),char(0),char(-44),char(0),
+char(14),char(0),char(-43),char(0),char(14),char(0),char(-42),char(0),char(14),char(0),char(-41),char(0),char(14),char(0),char(-40),char(0),char(0),char(0),char(-39),char(0),
+char(0),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),char(0),char(0),char(-36),char(0),char(4),char(0),char(-35),char(0),char(0),char(0),char(100),char(0),
+char(74),char(0),char(9),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),char(7),char(0),char(-75),char(0),
+char(7),char(0),char(-74),char(0),char(7),char(0),char(-73),char(0),char(7),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),
+char(75),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(8),char(0),char(-75),char(0),
+char(8),char(0),char(-74),char(0),char(8),char(0),char(-73),char(0),char(8),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),
+char(76),char(0),char(5),char(0),char(56),char(0),char(-95),char(0),char(13),char(0),char(-34),char(0),char(13),char(0),char(-33),char(0),char(7),char(0),char(-32),char(0),
+char(0),char(0),char(37),char(0),char(77),char(0),char(4),char(0),char(59),char(0),char(-95),char(0),char(14),char(0),char(-34),char(0),char(14),char(0),char(-33),char(0),
+char(8),char(0),char(-32),char(0),char(50),char(0),char(22),char(0),char(8),char(0),char(-31),char(0),char(8),char(0),char(-76),char(0),char(8),char(0),char(111),char(0),
+char(8),char(0),char(-30),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(-29),char(0),char(8),char(0),char(-28),char(0),char(8),char(0),char(-27),char(0),
+char(8),char(0),char(-26),char(0),char(8),char(0),char(-25),char(0),char(8),char(0),char(-24),char(0),char(8),char(0),char(-23),char(0),char(8),char(0),char(-22),char(0),
+char(8),char(0),char(-21),char(0),char(8),char(0),char(-20),char(0),char(8),char(0),char(-19),char(0),char(4),char(0),char(-18),char(0),char(4),char(0),char(-17),char(0),
+char(4),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),char(0),char(0),char(37),char(0),char(52),char(0),char(22),char(0),
+char(7),char(0),char(-31),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-30),char(0),char(7),char(0),char(113),char(0),
+char(7),char(0),char(-29),char(0),char(7),char(0),char(-28),char(0),char(7),char(0),char(-27),char(0),char(7),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0),
+char(7),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-22),char(0),char(7),char(0),char(-21),char(0),char(7),char(0),char(-20),char(0),
+char(7),char(0),char(-19),char(0),char(4),char(0),char(-18),char(0),char(4),char(0),char(-17),char(0),char(4),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),
+char(4),char(0),char(-14),char(0),char(0),char(0),char(37),char(0),char(78),char(0),char(4),char(0),char(7),char(0),char(-13),char(0),char(7),char(0),char(-12),char(0),
+char(7),char(0),char(-11),char(0),char(4),char(0),char(79),char(0),char(79),char(0),char(10),char(0),char(78),char(0),char(-10),char(0),char(13),char(0),char(-9),char(0),
+char(13),char(0),char(-8),char(0),char(13),char(0),char(-7),char(0),char(13),char(0),char(-6),char(0),char(13),char(0),char(-5),char(0),char(7),char(0),char(-120),char(0),
+char(7),char(0),char(-4),char(0),char(4),char(0),char(-3),char(0),char(4),char(0),char(53),char(0),char(80),char(0),char(4),char(0),char(78),char(0),char(-10),char(0),
+char(4),char(0),char(-2),char(0),char(7),char(0),char(-1),char(0),char(4),char(0),char(0),char(1),char(81),char(0),char(4),char(0),char(13),char(0),char(-5),char(0),
+char(78),char(0),char(-10),char(0),char(4),char(0),char(1),char(1),char(7),char(0),char(2),char(1),char(82),char(0),char(7),char(0),char(13),char(0),char(3),char(1),
+char(78),char(0),char(-10),char(0),char(4),char(0),char(4),char(1),char(7),char(0),char(5),char(1),char(7),char(0),char(6),char(1),char(7),char(0),char(7),char(1),
+char(4),char(0),char(53),char(0),char(83),char(0),char(6),char(0),char(15),char(0),char(8),char(1),char(13),char(0),char(6),char(1),char(13),char(0),char(9),char(1),
+char(58),char(0),char(10),char(1),char(4),char(0),char(11),char(1),char(7),char(0),char(7),char(1),char(84),char(0),char(26),char(0),char(4),char(0),char(12),char(1),
+char(7),char(0),char(13),char(1),char(7),char(0),char(-76),char(0),char(7),char(0),char(14),char(1),char(7),char(0),char(15),char(1),char(7),char(0),char(16),char(1),
+char(7),char(0),char(17),char(1),char(7),char(0),char(18),char(1),char(7),char(0),char(19),char(1),char(7),char(0),char(20),char(1),char(7),char(0),char(21),char(1),
+char(7),char(0),char(22),char(1),char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1),char(7),char(0),char(25),char(1),char(7),char(0),char(26),char(1),
+char(7),char(0),char(27),char(1),char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1),char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1),
+char(4),char(0),char(32),char(1),char(4),char(0),char(33),char(1),char(4),char(0),char(34),char(1),char(4),char(0),char(35),char(1),char(4),char(0),char(118),char(0),
+char(85),char(0),char(12),char(0),char(15),char(0),char(36),char(1),char(15),char(0),char(37),char(1),char(15),char(0),char(38),char(1),char(13),char(0),char(39),char(1),
+char(13),char(0),char(40),char(1),char(7),char(0),char(41),char(1),char(4),char(0),char(42),char(1),char(4),char(0),char(43),char(1),char(4),char(0),char(44),char(1),
+char(4),char(0),char(45),char(1),char(7),char(0),char(5),char(1),char(4),char(0),char(53),char(0),char(86),char(0),char(27),char(0),char(17),char(0),char(46),char(1),
+char(15),char(0),char(47),char(1),char(15),char(0),char(48),char(1),char(13),char(0),char(39),char(1),char(13),char(0),char(49),char(1),char(13),char(0),char(50),char(1),
+char(13),char(0),char(51),char(1),char(13),char(0),char(52),char(1),char(13),char(0),char(53),char(1),char(4),char(0),char(54),char(1),char(7),char(0),char(55),char(1),
+char(4),char(0),char(56),char(1),char(4),char(0),char(57),char(1),char(4),char(0),char(58),char(1),char(7),char(0),char(59),char(1),char(7),char(0),char(60),char(1),
+char(4),char(0),char(61),char(1),char(4),char(0),char(62),char(1),char(7),char(0),char(63),char(1),char(7),char(0),char(64),char(1),char(7),char(0),char(65),char(1),
+char(7),char(0),char(66),char(1),char(7),char(0),char(67),char(1),char(7),char(0),char(68),char(1),char(4),char(0),char(69),char(1),char(4),char(0),char(70),char(1),
+char(4),char(0),char(71),char(1),char(87),char(0),char(12),char(0),char(9),char(0),char(72),char(1),char(9),char(0),char(73),char(1),char(13),char(0),char(74),char(1),
+char(7),char(0),char(75),char(1),char(7),char(0),char(-27),char(0),char(7),char(0),char(76),char(1),char(4),char(0),char(77),char(1),char(13),char(0),char(78),char(1),
+char(4),char(0),char(79),char(1),char(4),char(0),char(80),char(1),char(4),char(0),char(81),char(1),char(4),char(0),char(53),char(0),char(88),char(0),char(19),char(0),
+char(48),char(0),char(126),char(0),char(85),char(0),char(82),char(1),char(78),char(0),char(83),char(1),char(79),char(0),char(84),char(1),char(80),char(0),char(85),char(1),
+char(81),char(0),char(86),char(1),char(82),char(0),char(87),char(1),char(83),char(0),char(88),char(1),char(86),char(0),char(89),char(1),char(87),char(0),char(90),char(1),
+char(4),char(0),char(91),char(1),char(4),char(0),char(57),char(1),char(4),char(0),char(92),char(1),char(4),char(0),char(93),char(1),char(4),char(0),char(94),char(1),
+char(4),char(0),char(95),char(1),char(4),char(0),char(96),char(1),char(4),char(0),char(97),char(1),char(84),char(0),char(98),char(1),};
 int sBulletDNAlen64= sizeof(sBulletDNAstr64);

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

@@ -4,8 +4,8 @@ Copyright (c) 2003-2009 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, 
+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.
@@ -32,12 +32,12 @@ extern int sBulletDNAlen;
 extern char sBulletDNAstr64[];
 extern int sBulletDNAlen64;
 
-SIMD_FORCE_INLINE	int btStrLen(const char* str) 
+SIMD_FORCE_INLINE	int btStrLen(const char* str)
 {
-    if (!str) 
+    if (!str)
 		return(0);
 	int len = 0;
-    
+
 	while (*str != 0)
 	{
         str++;
@@ -85,7 +85,7 @@ public:
 	virtual	void*	getUniquePointer(void*oldPtr) = 0;
 
 	virtual	void	startSerialization() = 0;
-	
+
 	virtual	void	finishSerialization() = 0;
 
 	virtual	const char*	findNameForPointer(const void* ptr) const = 0;
@@ -98,6 +98,9 @@ public:
 
 	virtual void	setSerializationFlags(int flags) = 0;
 
+	virtual int getNumChunks() const = 0;
+
+	virtual const btChunk* getChunk(int chunkIndex) const = 0;
 
 };
 
@@ -134,11 +137,35 @@ struct	btPointerUid
 	};
 };
 
+struct btBulletSerializedArrays
+{
+	btBulletSerializedArrays()
+	{
+	}
+	btAlignedObjectArray<struct btQuantizedBvhDoubleData*>	m_bvhsDouble;
+	btAlignedObjectArray<struct btQuantizedBvhFloatData*>	m_bvhsFloat;
+	btAlignedObjectArray<struct btCollisionShapeData*> m_colShapeData;
+	btAlignedObjectArray<struct btDynamicsWorldDoubleData*> m_dynamicWorldInfoDataDouble;
+	btAlignedObjectArray<struct btDynamicsWorldFloatData*> m_dynamicWorldInfoDataFloat;
+	btAlignedObjectArray<struct btRigidBodyDoubleData*> m_rigidBodyDataDouble;
+	btAlignedObjectArray<struct btRigidBodyFloatData*> m_rigidBodyDataFloat;
+	btAlignedObjectArray<struct btCollisionObjectDoubleData*> m_collisionObjectDataDouble;
+	btAlignedObjectArray<struct btCollisionObjectFloatData*> m_collisionObjectDataFloat;
+	btAlignedObjectArray<struct btTypedConstraintFloatData*> m_constraintDataFloat;
+	btAlignedObjectArray<struct btTypedConstraintDoubleData*> m_constraintDataDouble;
+	btAlignedObjectArray<struct btTypedConstraintData*> m_constraintData;//for backwards compatibility
+	btAlignedObjectArray<struct btSoftBodyFloatData*> m_softBodyFloatData;
+	btAlignedObjectArray<struct btSoftBodyDoubleData*> m_softBodyDoubleData;
+
+};
+
+
 ///The btDefaultSerializer is the main Bullet serialization class.
 ///The constructor takes an optional argument for backwards compatibility, it is recommended to leave this empty/zero.
 class btDefaultSerializer	:	public btSerializer
 {
 
+protected:
 
 	btAlignedObjectArray<char*>			mTypes;
 	btAlignedObjectArray<short*>			mStructs;
@@ -146,9 +173,9 @@ class btDefaultSerializer	:	public btSerializer
 	btHashMap<btHashInt, int>			mStructReverse;
 	btHashMap<btHashString,int>	mTypeLookup;
 
-	
+
 	btHashMap<btHashPtr,void*>	m_chunkP;
-	
+
 	btHashMap<btHashPtr,const char*>	m_nameMap;
 
 	btHashMap<btHashPtr,btPointerUid>	m_uniquePointers;
@@ -164,10 +191,10 @@ class btDefaultSerializer	:	public btSerializer
 
 
 	btAlignedObjectArray<btChunk*>	m_chunkPtrs;
-	
+
 protected:
 
-	virtual	void*	findPointer(void* oldPtr) 
+	virtual	void*	findPointer(void* oldPtr)
 	{
 		void** ptr = m_chunkP.find(oldPtr);
 		if (ptr && *ptr)
@@ -175,7 +202,7 @@ protected:
 		return 0;
 	}
 
-	
+
 
 
 
@@ -193,7 +220,7 @@ protected:
 			const int* valuePtr = mTypeLookup.find(key);
 			if (valuePtr)
 				return *valuePtr;
-			
+
 			return -1;
 		}
 
@@ -205,7 +232,7 @@ protected:
 
 			int littleEndian= 1;
 			littleEndian= ((char*)&littleEndian)[0];
-			
+
 
 			m_dna = btAlignedAlloc(dnalen,16);
 			memcpy(m_dna,bdnaOrg,dnalen);
@@ -233,16 +260,16 @@ protected:
 			// Parse names
 			if (!littleEndian)
 				*intPtr = btSwapEndian(*intPtr);
-				
+
 			dataLen = *intPtr;
-			
+
 			intPtr++;
 
 			cp = (char*)intPtr;
 			int i;
 			for ( i=0; i<dataLen; i++)
 			{
-				
+
 				while (*cp)cp++;
 				cp++;
 			}
@@ -260,11 +287,11 @@ protected:
 
 			if (!littleEndian)
 				*intPtr =  btSwapEndian(*intPtr);
-			
+
 			dataLen = *intPtr;
 			intPtr++;
 
-			
+
 			cp = (char*)intPtr;
 			for (i=0; i<dataLen; i++)
 			{
@@ -315,7 +342,7 @@ protected:
 
 			if (!littleEndian)
 				*intPtr = btSwapEndian(*intPtr);
-			dataLen = *intPtr ; 
+			dataLen = *intPtr ;
 			intPtr++;
 
 
@@ -323,7 +350,7 @@ protected:
 			for (i=0; i<dataLen; i++)
 			{
 				mStructs.push_back (shtPtr);
-				
+
 				if (!littleEndian)
 				{
 					shtPtr[0]= btSwapEndian(shtPtr[0]);
@@ -353,10 +380,10 @@ protected:
 			}
 		}
 
-public:	
-	
+public:
+
+
 
-	
 
 		btDefaultSerializer(int totalSize=0)
 			:m_totalSize(totalSize),
@@ -366,7 +393,7 @@ public:
 			m_serializationFlags(0)
 		{
 			m_buffer = m_totalSize?(unsigned char*)btAlignedAlloc(totalSize,16):0;
-			
+
 			const bool VOID_IS_8 = ((sizeof(void*)==8));
 
 #ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
@@ -385,7 +412,7 @@ public:
 				btAssert(0);
 #endif
 			}
-	
+
 #else //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
 			if (VOID_IS_8)
 			{
@@ -395,10 +422,10 @@ public:
 				initDNA((const char*)sBulletDNAstr,sBulletDNAlen);
 			}
 #endif //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
-	
+
 		}
 
-		virtual ~btDefaultSerializer() 
+		virtual ~btDefaultSerializer()
 		{
 			if (m_buffer)
 				btAlignedFree(m_buffer);
@@ -408,14 +435,14 @@ public:
 
 		void	writeHeader(unsigned char* buffer) const
 		{
-			
+
 
 #ifdef  BT_USE_DOUBLE_PRECISION
 			memcpy(buffer, "BULLETd", 7);
 #else
 			memcpy(buffer, "BULLETf", 7);
 #endif //BT_USE_DOUBLE_PRECISION
-	
+
 			int littleEndian= 1;
 			littleEndian= ((char*)&littleEndian)[0];
 
@@ -429,7 +456,7 @@ public:
 
 			if (littleEndian)
 			{
-				buffer[8]='v';				
+				buffer[8]='v';
 			} else
 			{
 				buffer[8]='V';
@@ -438,7 +465,7 @@ public:
 
 			buffer[9] = '2';
 			buffer[10] = '8';
-			buffer[11] = '2';
+			buffer[11] = '3';
 
 		}
 
@@ -450,7 +477,7 @@ public:
 				unsigned char* buffer = internalAlloc(BT_HEADER_LENGTH);
 				writeHeader(buffer);
 			}
-			
+
 		}
 
 		virtual	void	finishSerialization()
@@ -503,7 +530,7 @@ public:
 				return uptr->m_ptr;
 			}
 			m_uniqueIdGenerator++;
-			
+
 			btPointerUid uid;
 			uid.m_uniqueIds[0] = m_uniqueIdGenerator;
 			uid.m_uniqueIds[1] = m_uniqueIdGenerator;
@@ -530,17 +557,17 @@ public:
 			}
 
 			chunk->m_dna_nr = getReverseType(structType);
-			
+
 			chunk->m_chunkCode = chunkCode;
-			
+
 			void* uniquePtr = getUniquePointer(oldPtr);
-			
+
 			m_chunkP.insert(oldPtr,uniquePtr);//chunk->m_oldPtr);
 			chunk->m_oldPtr = uniquePtr;//oldPtr;
-			
+
 		}
 
-		
+
 		virtual unsigned char* internalAlloc(size_t size)
 		{
 			unsigned char* ptr = 0;
@@ -558,7 +585,7 @@ public:
 			return ptr;
 		}
 
-		
+
 
 		virtual	btChunk*	allocate(size_t size, int numElements)
 		{
@@ -566,15 +593,15 @@ public:
 			unsigned char* ptr = internalAlloc(int(size)*numElements+sizeof(btChunk));
 
 			unsigned char* data = ptr + sizeof(btChunk);
-			
+
 			btChunk* chunk = (btChunk*)ptr;
 			chunk->m_chunkCode = 0;
 			chunk->m_oldPtr = data;
 			chunk->m_length = int(size)*numElements;
 			chunk->m_number = numElements;
-			
+
 			m_chunkPtrs.push_back(chunk);
-			
+
 
 			return chunk;
 		}
@@ -631,9 +658,195 @@ public:
 		{
 			m_serializationFlags = flags;
 		}
+		int getNumChunks() const
+		{
+			return m_chunkPtrs.size();
+		}
 
+		const btChunk* getChunk(int chunkIndex) const
+		{
+			return m_chunkPtrs[chunkIndex];
+		}
 };
 
 
+///In general it is best to use btDefaultSerializer,
+///in particular when writing the data to disk or sending it over the network.
+///The btInMemorySerializer is experimental and only suitable in a few cases.
+///The btInMemorySerializer takes a shortcut and can be useful to create a deep-copy
+///of objects. There will be a demo on how to use the btInMemorySerializer.
+#ifdef ENABLE_INMEMORY_SERIALIZER
+
+struct btInMemorySerializer : public btDefaultSerializer
+{
+    btHashMap<btHashPtr,btChunk*> m_uid2ChunkPtr;
+    btHashMap<btHashPtr,void*> m_orgPtr2UniqueDataPtr;
+    btHashMap<btHashString,const void*> m_names2Ptr;
+    btHashMap<btHashPtr,void*> m_skipPointers;
+
+    btBulletSerializedArrays    m_arrays;
+
+
+    virtual void startSerialization()
+    {
+        m_uid2ChunkPtr.clear();
+        //todo: m_arrays.clear();
+        btDefaultSerializer::startSerialization();
+    }
+
+    btChunk* findChunkFromUniquePointer(void* uniquePointer)
+    {
+        btChunk** chkPtr = m_uid2ChunkPtr[uniquePointer];
+        if (chkPtr)
+        {
+            return *chkPtr;
+        }
+        return 0;
+    }
+
+	virtual	void	registerNameForPointer(const void* ptr, const char* name)
+    {
+       btDefaultSerializer::registerNameForPointer(ptr,name);
+       m_names2Ptr.insert(name,ptr);
+    }
+
+    virtual void finishSerialization()
+    {
+    }
+
+    virtual void* getUniquePointer(void*oldPtr)
+    {
+        if (oldPtr==0)
+            return 0;
+
+        // void* uniquePtr = getUniquePointer(oldPtr);
+        btChunk* chunk = findChunkFromUniquePointer(oldPtr);
+        if (chunk)
+        {
+            return chunk->m_oldPtr;
+        } else
+        {
+            const char* n = (const char*) oldPtr;
+            const void** ptr = m_names2Ptr[n];
+            if (ptr)
+            {
+                return oldPtr;
+            } else
+            {
+            		void** ptr2 = m_skipPointers[oldPtr];
+            		if (ptr2)
+								{
+									return 0;
+								} else
+								{
+									//If this assert hit, serialization happened in the wrong order
+									// 'getUniquePointer'
+									btAssert(0);
+								}
+
+            }
+            return 0;
+        }
+				return oldPtr;
+    }
+
+    virtual void finalizeChunk(btChunk* chunk, const char* structType, int chunkCode,void* oldPtr)
+    {
+        if (!(m_serializationFlags&BT_SERIALIZE_NO_DUPLICATE_ASSERT))
+        {
+            btAssert(!findPointer(oldPtr));
+        }
+
+        chunk->m_dna_nr = getReverseType(structType);
+        chunk->m_chunkCode = chunkCode;
+        //void* uniquePtr = getUniquePointer(oldPtr);
+        m_chunkP.insert(oldPtr,oldPtr);//chunk->m_oldPtr);
+        // chunk->m_oldPtr = uniquePtr;//oldPtr;
+
+        void* uid = findPointer(oldPtr);
+        m_uid2ChunkPtr.insert(uid,chunk);
+
+        switch (chunk->m_chunkCode)
+			{
+			case BT_SOFTBODY_CODE:
+			{
+	#ifdef BT_USE_DOUBLE_PRECISION
+					m_arrays.m_softBodyDoubleData.push_back((btSoftBodyDoubleData*) chunk->m_oldPtr);
+	#else
+					m_arrays.m_softBodyFloatData.push_back((btSoftBodyFloatData*) chunk->m_oldPtr);
+	#endif
+					break;
+				}
+			case BT_COLLISIONOBJECT_CODE:
+				{
+	#ifdef BT_USE_DOUBLE_PRECISION
+					m_arrays.m_collisionObjectDataDouble.push_back((btCollisionObjectDoubleData*)chunk->m_oldPtr);
+	#else//BT_USE_DOUBLE_PRECISION
+					m_arrays.m_collisionObjectDataFloat.push_back((btCollisionObjectFloatData*)chunk->m_oldPtr);
+	#endif //BT_USE_DOUBLE_PRECISION
+					break;
+				}
+			case BT_RIGIDBODY_CODE:
+				{
+	#ifdef BT_USE_DOUBLE_PRECISION
+					m_arrays.m_rigidBodyDataDouble.push_back((btRigidBodyDoubleData*)chunk->m_oldPtr);
+	#else
+					m_arrays.m_rigidBodyDataFloat.push_back((btRigidBodyFloatData*)chunk->m_oldPtr);
+	#endif//BT_USE_DOUBLE_PRECISION
+					break;
+				};
+			case BT_CONSTRAINT_CODE:
+				{
+	#ifdef BT_USE_DOUBLE_PRECISION
+					m_arrays.m_constraintDataDouble.push_back((btTypedConstraintDoubleData*)chunk->m_oldPtr);
+	#else
+					m_arrays.m_constraintDataFloat.push_back((btTypedConstraintFloatData*)chunk->m_oldPtr);
+	#endif
+					break;
+				}
+			case BT_QUANTIZED_BVH_CODE:
+				{
+	#ifdef BT_USE_DOUBLE_PRECISION
+					m_arrays.m_bvhsDouble.push_back((btQuantizedBvhDoubleData*) chunk->m_oldPtr);
+	#else
+					m_arrays.m_bvhsFloat.push_back((btQuantizedBvhFloatData*) chunk->m_oldPtr);
+	#endif
+					break;
+				}
+
+			case BT_SHAPE_CODE:
+				{
+					btCollisionShapeData* shapeData = (btCollisionShapeData*) chunk->m_oldPtr;
+					m_arrays.m_colShapeData.push_back(shapeData);
+					break;
+				}
+			case BT_TRIANLGE_INFO_MAP:
+			case BT_ARRAY_CODE:
+			case BT_SBMATERIAL_CODE:
+			case BT_SBNODE_CODE:
+			case BT_DYNAMICSWORLD_CODE:
+			case BT_DNA_CODE:
+				{
+					break;
+				}
+			default:
+				{
+				}
+			};
+    }
+
+    int getNumChunks() const
+    {
+        return m_uid2ChunkPtr.size();
+    }
+
+    const btChunk* getChunk(int chunkIndex) const
+    {
+        return *m_uid2ChunkPtr.getAtIndex(chunkIndex);
+    }
+
+};
+#endif //ENABLE_INMEMORY_SERIALIZER
+
 #endif //BT_SERIALIZER_H
 

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

@@ -63,7 +63,7 @@ long _maxdot_large( const float *vv, const float *vec, unsigned long count, floa
     float4 stack_array[ STACK_ARRAY_COUNT ];
     
 #if DEBUG
-    memset( stack_array, -1, STACK_ARRAY_COUNT * sizeof(stack_array[0]) );
+    //memset( stack_array, -1, STACK_ARRAY_COUNT * sizeof(stack_array[0]) );
 #endif
     
     size_t index;
@@ -448,7 +448,7 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa
     float4 stack_array[ STACK_ARRAY_COUNT ];
     
 #if DEBUG
-    memset( stack_array, -1, STACK_ARRAY_COUNT * sizeof(stack_array[0]) );
+    //memset( stack_array, -1, STACK_ARRAY_COUNT * sizeof(stack_array[0]) );
 #endif
     
     size_t index;

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

@@ -297,7 +297,7 @@ public:
 	SIMD_FORCE_INLINE btVector3& normalize() 
 	{
 		
-		btAssert(length() != btScalar(0));
+		btAssert(!fuzzyZero());
 
 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)		
         // dot product first
@@ -501,10 +501,10 @@ public:
 		__m128 tmp3 = _mm_add_ps(r0,r1);
 		mVec128 = tmp3;
 #elif defined(BT_USE_NEON)
-		mVec128 = vsubq_f32(v1.mVec128, v0.mVec128);
-		mVec128 = vmulq_n_f32(mVec128, rt);
-		mVec128 = vaddq_f32(mVec128, v0.mVec128);
-#else	
+		float32x4_t vl = vsubq_f32(v1.mVec128, v0.mVec128);
+		vl = vmulq_n_f32(vl, rt);
+		mVec128 = vaddq_f32(vl, v0.mVec128);
+#else
 		btScalar s = btScalar(1.0) - rt;
 		m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
 		m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
@@ -685,9 +685,10 @@ public:
 		return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
 	}
 
+
 	SIMD_FORCE_INLINE bool fuzzyZero() const 
 	{
-		return length2() < SIMD_EPSILON;
+		return length2() < SIMD_EPSILON*SIMD_EPSILON;
 	}
 
 	SIMD_FORCE_INLINE	void	serialize(struct	btVector3Data& dataOut) const;
@@ -950,9 +951,9 @@ SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
 
 SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
 {
-	btVector3 norm = *this;
+	btVector3 nrm = *this;
 
-	return norm.normalize();
+	return nrm.normalize();
 } 
 
 SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const
@@ -1010,21 +1011,21 @@ SIMD_FORCE_INLINE   long    btVector3::maxDot( const btVector3 *array, long arra
     if( array_count < scalar_cutoff )	
 #endif
     {
-        btScalar maxDot = -SIMD_INFINITY;
+        btScalar maxDot1 = -SIMD_INFINITY;
         int i = 0;
         int ptIndex = -1;
         for( i = 0; i < array_count; i++ )
         {
             btScalar dot = array[i].dot(*this);
             
-            if( dot > maxDot )
+            if( dot > maxDot1 )
             {
-                maxDot = dot;
+                maxDot1 = dot;
                 ptIndex = i;
             }
         }
         
-        dotOut = maxDot;
+        dotOut = maxDot1;
         return ptIndex;
     }
 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)